diff --git a/CppUnitLite/Failure.h b/CppUnitLite/Failure.h index 93c99df12..67ac5ba38 100644 --- a/CppUnitLite/Failure.h +++ b/CppUnitLite/Failure.h @@ -29,42 +29,42 @@ class Failure { public: - Failure (const std::string& theTestName, - const std::string& theFileName, - long theLineNumber, - const std::string& theCondition) - : message (theCondition), - testName (theTestName), - fileName (theFileName), - lineNumber (theLineNumber) - { - } + Failure (const std::string& theTestName, + const std::string& theFileName, + long theLineNumber, + const std::string& theCondition) + : message (theCondition), + testName (theTestName), + fileName (theFileName), + lineNumber (theLineNumber) + { + } - Failure (const std::string& theTestName, - const std::string& theFileName, - const std::string& theCondition) - : message (theCondition), - testName (theTestName), - fileName (theFileName), - lineNumber (-1) - { - } + Failure (const std::string& theTestName, + const std::string& theFileName, + const std::string& theCondition) + : message (theCondition), + testName (theTestName), + fileName (theFileName), + lineNumber (-1) + { + } - Failure (const std::string& theTestName, - const std::string& theFileName, - long theLineNumber, - const std::string& expected, - const std::string& actual) - : message("expected " + expected + " but was: " + actual), - testName (theTestName), - fileName (theFileName), - lineNumber (theLineNumber) - { - } + Failure (const std::string& theTestName, + const std::string& theFileName, + long theLineNumber, + const std::string& expected, + const std::string& actual) + : message("expected " + expected + " but was: " + actual), + testName (theTestName), + fileName (theFileName), + lineNumber (theLineNumber) + { + } - std::string message; - std::string testName; - std::string fileName; - long lineNumber; + std::string message; + std::string testName; + std::string fileName; + long lineNumber; }; diff --git a/CppUnitLite/Test.cpp b/CppUnitLite/Test.cpp index e6a2ad51f..481fceb07 100644 --- a/CppUnitLite/Test.cpp +++ b/CppUnitLite/Test.cpp @@ -18,58 +18,58 @@ #include Test::Test (const std::string& testName) - : name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true) + : name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true) { - TestRegistry::addTest (this); + TestRegistry::addTest (this); } Test::Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck) - : name_(testName), next_(0), filename_(filename), lineNumber_(lineNumber), safeCheck_(safeCheck) + : name_(testName), next_(0), filename_(filename), lineNumber_(lineNumber), safeCheck_(safeCheck) { - TestRegistry::addTest (this); + TestRegistry::addTest (this); } Test *Test::getNext() const { - return next_; + return next_; } void Test::setNext(Test *test) -{ - next_ = test; +{ + next_ = test; } bool Test::check(long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber) { - if (expected == actual) - return true; - result.addFailure ( - Failure ( - name_, - boost::lexical_cast (__FILE__), - __LINE__, - boost::lexical_cast (expected), - boost::lexical_cast (actual))); + if (expected == actual) + return true; + result.addFailure ( + Failure ( + name_, + boost::lexical_cast (__FILE__), + __LINE__, + boost::lexical_cast (expected), + boost::lexical_cast (actual))); - return false; + return false; } bool Test::check(const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber) { - if (expected == actual) - return true; - result.addFailure ( - Failure ( - name_, - boost::lexical_cast (__FILE__), - __LINE__, - expected, - actual)); + if (expected == actual) + return true; + result.addFailure ( + Failure ( + name_, + boost::lexical_cast (__FILE__), + __LINE__, + expected, + actual)); - return false; + return false; } diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 820ed48cf..52b79f65c 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -30,30 +30,30 @@ class TestResult; class Test { public: - Test (const std::string& testName); - Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck); + Test (const std::string& testName); + Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck); virtual ~Test() {}; - virtual void run (TestResult& result) = 0; + virtual void run (TestResult& result) = 0; - void setNext(Test *test); - Test *getNext () const; - std::string getName() const {return name_;} - std::string getFilename() const {return filename_;} - long getLineNumber() const {return lineNumber_;} - bool safe() const {return safeCheck_;} + void setNext(Test *test); + Test *getNext () const; + std::string getName() const {return name_;} + std::string getFilename() const {return filename_;} + long getLineNumber() const {return lineNumber_;} + bool safe() const {return safeCheck_;} protected: - bool check (long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber); - bool check (const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber); + bool check (long expected, long actual, TestResult& result, const std::string& fileName, long lineNumber); + bool check (const std::string& expected, const std::string& actual, TestResult& result, const std::string& fileName, long lineNumber); - std::string name_; - Test *next_; - std::string filename_; - long lineNumber_; /// This is the line line number of the test, rather than the a single check - bool safeCheck_; + std::string name_; + Test *next_; + std::string filename_; + long lineNumber_; /// This is the line line number of the test, rather than the a single check + bool safeCheck_; }; @@ -62,11 +62,11 @@ protected: */ #define TEST(testName, testGroup)\ class testGroup##testName##Test : public Test \ - { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ + { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ virtual ~testGroup##testName##Test () {};\ void run (TestResult& result_);} \ testGroup##testName##Instance; \ - void testGroup##testName##Test::run (TestResult& result_) + void testGroup##testName##Test::run (TestResult& result_) /** * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this @@ -74,11 +74,11 @@ protected: */ #define TEST_UNSAFE(testName, testGroup)\ class testGroup##testName##Test : public Test \ - { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ - virtual ~testGroup##testName##Test () {};\ + { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ + virtual ~testGroup##testName##Test () {};\ void run (TestResult& result_);} \ testGroup##testName##Instance; \ - void testGroup##testName##Test::run (TestResult& result_) + void testGroup##testName##Test::run (TestResult& result_) /* * Convention for tests: @@ -100,18 +100,18 @@ protected: #define THROWS_EXCEPTION(condition)\ { try { condition; \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ - return; } \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ + return; } \ catch (...) {} } #define CHECK_EXCEPTION(condition, exception_name)\ { try { condition; \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ - return; } \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ + return; } \ catch (exception_name&) {} \ catch (...) { \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast(#condition) + boost::lexical_cast(", expected: ") + boost::lexical_cast(#exception_name))); \ - return; } } + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast(#condition) + boost::lexical_cast(", expected: ") + boost::lexical_cast(#exception_name))); \ + return; } } #define EQUALITY(expected,actual)\ { if (!assert_equal(expected,actual)) \ diff --git a/CppUnitLite/TestRegistry.cpp b/CppUnitLite/TestRegistry.cpp index 1d06b225c..b493e81a6 100644 --- a/CppUnitLite/TestRegistry.cpp +++ b/CppUnitLite/TestRegistry.cpp @@ -20,64 +20,64 @@ void TestRegistry::addTest (Test *test) { - instance ().add (test); + instance ().add (test); } int TestRegistry::runAllTests (TestResult& result) { - return instance ().run (result); + return instance ().run (result); } TestRegistry& TestRegistry::instance () { - static TestRegistry registry; - return registry; + static TestRegistry registry; + return registry; } void TestRegistry::add (Test *test) { - if (tests == 0) { - test->setNext(0); - tests = test; - lastTest = test; - return; - } + if (tests == 0) { + test->setNext(0); + tests = test; + lastTest = test; + return; + } - test->setNext (0); - lastTest->setNext(test); - lastTest = test; + test->setNext (0); + lastTest->setNext(test); + lastTest = test; } int TestRegistry::run (TestResult& result) { - result.testsStarted (); + result.testsStarted (); - for (Test *test = tests; test != 0; test = test->getNext ()) { - if (test->safe()) { - try { - test->run (result); - } catch (std::exception& e) { - // catch standard exceptions and derivatives - result.addFailure( - Failure(test->getName(), test->getFilename(), test->getLineNumber(), - std::string("Exception: ") + std::string(e.what()))); - } catch (...) { - // catch all other exceptions - result.addFailure( - Failure(test->getName(), test->getFilename(), test->getLineNumber(), - "ExceptionThrown!")); - } - } - else { - test->run (result); - } - } - result.testsEnded (); - return result.getFailureCount(); + for (Test *test = tests; test != 0; test = test->getNext ()) { + if (test->safe()) { + try { + test->run (result); + } catch (std::exception& e) { + // catch standard exceptions and derivatives + result.addFailure( + Failure(test->getName(), test->getFilename(), test->getLineNumber(), + std::string("Exception: ") + std::string(e.what()))); + } catch (...) { + // catch all other exceptions + result.addFailure( + Failure(test->getName(), test->getFilename(), test->getLineNumber(), + "ExceptionThrown!")); + } + } + else { + test->run (result); + } + } + result.testsEnded (); + return result.getFailureCount(); } diff --git a/CppUnitLite/TestRegistry.h b/CppUnitLite/TestRegistry.h index 4053e815c..56db991ad 100644 --- a/CppUnitLite/TestRegistry.h +++ b/CppUnitLite/TestRegistry.h @@ -29,18 +29,18 @@ class TestResult; class TestRegistry { public: - static void addTest (Test *test); - static int runAllTests (TestResult& result); + static void addTest (Test *test); + static int runAllTests (TestResult& result); private: - static TestRegistry& instance (); - void add (Test *test); - int run (TestResult& result); + static TestRegistry& instance (); + void add (Test *test); + int run (TestResult& result); - - Test *tests; - Test *lastTest; + + Test *tests; + Test *lastTest; }; diff --git a/CppUnitLite/TestResult.cpp b/CppUnitLite/TestResult.cpp index a9e599989..2519c94a9 100644 --- a/CppUnitLite/TestResult.cpp +++ b/CppUnitLite/TestResult.cpp @@ -17,7 +17,7 @@ TestResult::TestResult () - : failureCount (0) + : failureCount (0) { } @@ -29,29 +29,29 @@ void TestResult::testsStarted () void TestResult::addFailure (const Failure& failure) { - if (failure.lineNumber < 0) // allow for no line number - fprintf (stdout, "%s%s%s%s\n", - "Failure: \"", - failure.message.c_str (), - "\" in ", - failure.fileName.c_str ()); - else - fprintf (stdout, "%s%s%ld%s%s%s\n", - failure.fileName.c_str(), // Format matches Eclipse error flagging - ":", - failure.lineNumber, - ": Failure: \"", - failure.message.c_str(), - "\" "); + if (failure.lineNumber < 0) // allow for no line number + fprintf (stdout, "%s%s%s%s\n", + "Failure: \"", + failure.message.c_str (), + "\" in ", + failure.fileName.c_str ()); + else + fprintf (stdout, "%s%s%ld%s%s%s\n", + failure.fileName.c_str(), // Format matches Eclipse error flagging + ":", + failure.lineNumber, + ": Failure: \"", + failure.message.c_str(), + "\" "); - failureCount++; + failureCount++; } void TestResult::testsEnded () { - if (failureCount > 0) - fprintf (stdout, "There were %d failures\n", failureCount); - else - fprintf (stdout, "There were no test failures\n"); + if (failureCount > 0) + fprintf (stdout, "There were %d failures\n", failureCount); + else + fprintf (stdout, "There were no test failures\n"); } diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h index e5f5abfb1..a30275647 100644 --- a/CppUnitLite/TestResult.h +++ b/CppUnitLite/TestResult.h @@ -28,16 +28,16 @@ class Failure; class TestResult { public: - TestResult (); + TestResult (); virtual ~TestResult() {}; - virtual void testsStarted (); - virtual void addFailure (const Failure& failure); - virtual void testsEnded (); + virtual void testsStarted (); + virtual void addFailure (const Failure& failure); + virtual void testsEnded (); int getFailureCount() {return failureCount;} private: - int failureCount; + int failureCount; }; #endif diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 4133d1e1e..e024d832c 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file CameraResectioning.cpp + * @file CameraResectioning.cpp * @brief An example of gtsam for solving the camera resectioning problem - * @author Duy-Nguyen Ta - * @date Aug 23, 2011 + * @author Duy-Nguyen Ta + * @date Aug 23, 2011 */ #include @@ -30,27 +30,27 @@ using symbol_shorthand::X; * a known 3D point in the image */ class ResectioningFactor: public NoiseModelFactor1 { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; - shared_ptrK K_; // camera's intrinsic parameters - Point3 P_; // 3D point on the calibration rig - Point2 p_; // 2D measurement of the 3D point + shared_ptrK K_; // camera's intrinsic parameters + Point3 P_; // 3D point on the calibration rig + Point2 p_; // 2D measurement of the 3D point public: - /// Construct factor given known point P and its projection p - ResectioningFactor(const SharedNoiseModel& model, const Key& key, - const shared_ptrK& calib, const Point2& p, const Point3& P) : - Base(model, key), K_(calib), P_(P), p_(p) { - } + /// Construct factor given known point P and its projection p + ResectioningFactor(const SharedNoiseModel& model, const Key& key, + const shared_ptrK& calib, const Point2& p, const Point3& P) : + Base(model, key), K_(calib), P_(P), p_(p) { + } - /// evaluate the error - virtual Vector evaluateError(const Pose3& pose, boost::optional H = - boost::none) const { - SimpleCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(P_, H) - p_); - return reprojectionError.vector(); - } + /// evaluate the error + virtual Vector evaluateError(const Pose3& pose, boost::optional H = + boost::none) const { + SimpleCamera camera(pose, *K_); + Point2 reprojectionError(camera.project(P_, H) - p_); + return reprojectionError.vector(); + } }; /******************************************************************************* @@ -62,37 +62,37 @@ public: * 2D Point: (55,45) (45,45) (45,55) (55,55) *******************************************************************************/ int main(int argc, char* argv[]) { - /* read camera intrinsic parameters */ - shared_ptrK calib(new Cal3_S2(1, 1, 0, 50, 50)); + /* read camera intrinsic parameters */ + shared_ptrK calib(new Cal3_S2(1, 1, 0, 50, 50)); - /* 1. create graph */ - NonlinearFactorGraph graph; + /* 1. create graph */ + NonlinearFactorGraph graph; - /* 2. add factors to the graph */ - // add measurement factors - SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5)); - boost::shared_ptr factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + /* 2. add factors to the graph */ + // add measurement factors + SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5)); + boost::shared_ptr factor; + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0))); + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0))); + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0))); + graph.push_back( + boost::make_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0))); - /* 3. Create an initial estimate for the camera pose */ - Values initial; - initial.insert(X(1), - Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2))); + /* 3. Create an initial estimate for the camera pose */ + Values initial; + initial.insert(X(1), + Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2))); - /* 4. Optimize the graph using Levenberg-Marquardt*/ - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Final result:\n"); + /* 4. Optimize the graph using Levenberg-Marquardt*/ + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("Final result:\n"); - return 0; + return 0; } diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 6572236a0..64c17c3db 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file DiscreteBayesNet_FG.cpp - * @brief Discrete Bayes Net example using Factor Graphs - * @author Abhijit - * @date Jun 4, 2012 + * @file DiscreteBayesNet_FG.cpp + * @brief Discrete Bayes Net example using Factor Graphs + * @author Abhijit + * @date Jun 4, 2012 * * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529] * You may be familiar with other graphical model packages like BNT (available @@ -31,89 +31,89 @@ using namespace gtsam; int main(int argc, char **argv) { - // We assume binary state variables - // we have 0 == "False" and 1 == "True" - const size_t nrStates = 2; + // We assume binary state variables + // we have 0 == "False" and 1 == "True" + const size_t nrStates = 2; - // define variables - DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates), - WetGrass(4, nrStates); + // define variables + DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates), + WetGrass(4, nrStates); - // create Factor Graph of the bayes net - DiscreteFactorGraph graph; + // create Factor Graph of the bayes net + DiscreteFactorGraph graph; - // add factors - graph.add(Cloudy, "0.5 0.5"); //P(Cloudy) - graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy) - graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy) - graph.add(Sprinkler & Rain & WetGrass, - "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain) + // add factors + graph.add(Cloudy, "0.5 0.5"); //P(Cloudy) + graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy) + graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy) + graph.add(Sprinkler & Rain & WetGrass, + "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain) - // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional - // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp) + // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional + // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp) - // Since this is a relatively small distribution, we can as well print - // the whole distribution.. - cout << "Distribution of Example: " << endl; - cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10) - << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)" - << endl; - for (size_t a = 0; a < nrStates; a++) - for (size_t m = 0; m < nrStates; m++) - for (size_t h = 0; h < nrStates; h++) - for (size_t c = 0; c < nrStates; c++) { - DiscreteFactor::Values values; - values[Cloudy.first] = c; - values[Sprinkler.first] = h; - values[Rain.first] = m; - values[WetGrass.first] = a; - double prodPot = graph(values); - cout << boolalpha << setw(8) << (bool) c << setw(14) - << (bool) h << setw(12) << (bool) m << setw(13) - << (bool) a << setw(16) << prodPot << endl; - } + // Since this is a relatively small distribution, we can as well print + // the whole distribution.. + cout << "Distribution of Example: " << endl; + cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10) + << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)" + << endl; + for (size_t a = 0; a < nrStates; a++) + for (size_t m = 0; m < nrStates; m++) + for (size_t h = 0; h < nrStates; h++) + for (size_t c = 0; c < nrStates; c++) { + DiscreteFactor::Values values; + values[Cloudy.first] = c; + values[Sprinkler.first] = h; + values[Rain.first] = m; + values[WetGrass.first] = a; + double prodPot = graph(values); + cout << boolalpha << setw(8) << (bool) c << setw(14) + << (bool) h << setw(12) << (bool) m << setw(13) + << (bool) a << setw(16) << prodPot << endl; + } - // "Most Probable Explanation", i.e., configuration with largest value - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - cout <<"\nMost Probable Explanation (MPE):" << endl; - cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first] - << " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first] - << " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first] - << " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl; + // "Most Probable Explanation", i.e., configuration with largest value + DiscreteSequentialSolver solver(graph); + DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + cout <<"\nMost Probable Explanation (MPE):" << endl; + cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first] + << " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first] + << " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first] + << " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl; - // "Inference" We show an inference query like: probability that the Sprinkler was on; - // given that the grass is wet i.e. P( S | W=1) =? - cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl; + // "Inference" We show an inference query like: probability that the Sprinkler was on; + // given that the grass is wet i.e. P( S | W=1) =? + cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl; - // Method 1: we can compute the joint marginal P(S,W) and from that we can compute - // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps.. + // Method 1: we can compute the joint marginal P(S,W) and from that we can compute + // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps.. - //Step1: Compute P(S,W) - DiscreteFactorGraph jointFG; - jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices()); - DecisionTreeFactor probSW = jointFG.product(); + //Step1: Compute P(S,W) + DiscreteFactorGraph jointFG; + jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices()); + DecisionTreeFactor probSW = jointFG.product(); - //Step2: Compute P(W) - DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first); + //Step2: Compute P(W) + DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first); - //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1) - DiscreteFactor::Values values; - values[WetGrass.first] = 1; + //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1) + DiscreteFactor::Values values; + values[WetGrass.first] = 1; - //print P(S=0|W=1) - values[Sprinkler.first] = 0; - cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl; + //print P(S=0|W=1) + values[Sprinkler.first] = 0; + cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl; - //print P(S=1|W=1) - values[Sprinkler.first] = 1; - cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl; + //print P(S=1|W=1) + values[Sprinkler.first] = 1; + cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl; - // TODO: Method 2 : One way is to modify the factor graph to - // incorporate the evidence node and compute the marginal - // TODO: graph.addEvidence(Cloudy,0); + // TODO: Method 2 : One way is to modify the factor graph to + // incorporate the evidence node and compute the marginal + // TODO: graph.addEvidence(Cloudy,0); - return 0; + return 0; } diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index c6f6b636f..88b8da3e0 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -72,71 +72,71 @@ using namespace gtsam; int main(int argc, char** argv) { // Create a factor graph - NonlinearFactorGraph graph; + NonlinearFactorGraph graph; - // Create the keys we need for this simple example - static Symbol x1('x',1), x2('x',2), x3('x',3); - static Symbol l1('l',1), l2('l',2); + // Create the keys we need for this simple example + static Symbol x1('x',1), x2('x',2), x3('x',3); + static Symbol l1('l',1), l2('l',2); - // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) - Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph - // Add two odometry factors - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + // Add two odometry factors + Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); - // Add Range-Bearing measurements to two different landmarks - // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range - // create the measurement values - indices are (pose id, landmark id) - Rot2 bearing11 = Rot2::fromDegrees(45), - bearing21 = Rot2::fromDegrees(90), - bearing32 = Rot2::fromDegrees(90); - double range11 = std::sqrt(4.0+4.0), - range21 = 2.0, - range32 = 2.0; + // Add Range-Bearing measurements to two different landmarks + // create a noise model for the landmark measurements + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + // create the measurement values - indices are (pose id, landmark id) + Rot2 bearing11 = Rot2::fromDegrees(45), + bearing21 = Rot2::fromDegrees(90), + bearing32 = Rot2::fromDegrees(90); + double range11 = std::sqrt(4.0+4.0), + range21 = 2.0, + range32 = 2.0; - // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + // Add Bearing-Range factors + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); - // Print - graph.print("Factor Graph:\n"); + // Print + graph.print("Factor Graph:\n"); - // Create (deliberately inaccurate) initial estimate - Values initialEstimate; - initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insert(l1, Point2(1.8, 2.1)); - initialEstimate.insert(l2, Point2(4.1, 1.8)); + // Create (deliberately inaccurate) initial estimate + Values initialEstimate; + initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insert(l1, Point2(1.8, 2.1)); + initialEstimate.insert(l2, Point2(4.1, 1.8)); - // Print - initialEstimate.print("Initial Estimate:\n"); + // Print + initialEstimate.print("Initial Estimate:\n"); - // Optimize using Levenberg-Marquardt optimization. The optimizer - // accepts an optional set of configuration parameters, controlling - // things like convergence criteria, the type of linear system solver - // to use, and the amount of information displayed during optimization. - // Here we will use the default set of parameters. See the - // documentation for the full set of parameters. - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); + // Optimize using Levenberg-Marquardt optimization. The optimizer + // accepts an optional set of configuration parameters, controlling + // things like convergence criteria, the type of linear system solver + // to use, and the amount of information displayed during optimization. + // Here we will use the default set of parameters. See the + // documentation for the full set of parameters. + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); // Calculate and print marginal covariances for all variables - Marginals marginals(graph, result); + Marginals marginals(graph, result); print(marginals.marginalCovariance(x1), "x1 covariance"); print(marginals.marginalCovariance(x2), "x2 covariance"); print(marginals.marginalCovariance(x3), "x3 covariance"); print(marginals.marginalCovariance(l1), "l1 covariance"); print(marginals.marginalCovariance(l2), "l2 covariance"); - return 0; + return 0; } diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 4885e72c8..cbe97ae12 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -66,54 +66,54 @@ using namespace gtsam; int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it - NonlinearFactorGraph graph; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // 2a. Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + // 2a. Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); - // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // For simplicity, we will use the same noise model for odometry and loop closures + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - // 2b. Add odometry factors - // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + // 2b. Add odometry factors + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); + graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); + graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); - // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. We will use another Between Factor to enforce this constraint: - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); - graph.print("\nFactor Graph:\n"); // print + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. We will use another Between Factor to enforce this constraint: + graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.print("\nFactor Graph:\n"); // print - // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values - Values initialEstimate; - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); - initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); - initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial Estimate:\n"); // print + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer // The optimizer accepts an optional set of configuration parameters, - // controlling things like convergence criteria, the type of linear - // system solver to use, and the amount of information displayed during - // optimization. We will set a few parameters as a demonstration. - GaussNewtonParams parameters; - // Stop iterating once the change in error between steps is less than this value - parameters.relativeErrorTol = 1e-5; - // Do not perform more than N iteration steps - parameters.maxIterations = 100; - // Create the optimizer ... - GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); - // ... and optimize + // controlling things like convergence criteria, the type of linear + // system solver to use, and the amount of information displayed during + // optimization. We will set a few parameters as a demonstration. + GaussNewtonParams parameters; + // Stop iterating once the change in error between steps is less than this value + parameters.relativeErrorTol = 1e-5; + // Do not perform more than N iteration steps + parameters.maxIterations = 100; + // Create the optimizer ... + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + // ... and optimize Values result = optimizer.optimize(); result.print("Final Result:\n"); @@ -126,5 +126,5 @@ int main(int argc, char** argv) { cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl; cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl; - return 0; + return 0; } diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index cc873eb08..000a071da 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -31,26 +31,26 @@ using namespace gtsam; int main(int argc, char** argv) { - // Read File and create graph and initial estimate - // we are in build/examples, data is in examples/Data - NonlinearFactorGraph::shared_ptr graph ; - Values::shared_ptr initial; - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); - boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); - initial->print("Initial estimate:\n"); + // Read File and create graph and initial estimate + // we are in build/examples, data is in examples/Data + NonlinearFactorGraph::shared_ptr graph ; + Values::shared_ptr initial; + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); + boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); + initial->print("Initial estimate:\n"); - // Add a Gaussian prior on first poses - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); - graph->add(PriorFactor(0, priorMean, priorNoise)); + // Add a Gaussian prior on first poses + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); + graph->add(PriorFactor(0, priorMean, priorNoise)); - // Single Step Optimization using Levenberg-Marquardt - Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); - result.print("\nFinal result:\n"); + // Single Step Optimization using Levenberg-Marquardt + Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + result.print("\nFinal result:\n"); - // Plot the covariance of the last pose - Marginals marginals(*graph, result); - cout.precision(2); + // Plot the covariance of the last pose + Marginals marginals(*graph, result); + cout.precision(2); cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl; return 0; diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index cff9d754e..54c64e820 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -60,69 +60,69 @@ const double degree = M_PI / 180; int main() { - /** - * Step 1: Create a factor to express a unary constraint - * The "prior" in this case is the measurement from a sensor, - * with a model of the noise on the measurement. - * - * The "Key" created here is a label used to associate parts of the - * state (stored in "RotValues") with particular factors. They require - * an index to allow for lookup, and should be unique. - * - * In general, creating a factor requires: - * - A key or set of keys labeling the variables that are acted upon - * - A measurement value - * - A measurement model with the correct dimensionality for the factor - */ - Rot2 prior = Rot2::fromAngle(30 * degree); - prior.print("goal angle"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Symbol key('x',1); - PriorFactor factor(key, prior, model); + /** + * Step 1: Create a factor to express a unary constraint + * The "prior" in this case is the measurement from a sensor, + * with a model of the noise on the measurement. + * + * The "Key" created here is a label used to associate parts of the + * state (stored in "RotValues") with particular factors. They require + * an index to allow for lookup, and should be unique. + * + * In general, creating a factor requires: + * - A key or set of keys labeling the variables that are acted upon + * - A measurement value + * - A measurement model with the correct dimensionality for the factor + */ + Rot2 prior = Rot2::fromAngle(30 * degree); + prior.print("goal angle"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); + Symbol key('x',1); + PriorFactor factor(key, prior, model); - /** - * Step 2: Create a graph container and add the factor to it - * Before optimizing, all factors need to be added to a Graph container, - * which provides the necessary top-level functionality for defining a - * system of constraints. - * - * In this case, there is only one factor, but in a practical scenario, - * many more factors would be added. - */ - NonlinearFactorGraph graph; - graph.add(factor); - graph.print("full graph"); + /** + * Step 2: Create a graph container and add the factor to it + * Before optimizing, all factors need to be added to a Graph container, + * which provides the necessary top-level functionality for defining a + * system of constraints. + * + * In this case, there is only one factor, but in a practical scenario, + * many more factors would be added. + */ + NonlinearFactorGraph graph; + graph.add(factor); + graph.print("full graph"); - /** - * Step 3: Create an initial estimate - * An initial estimate of the solution for the system is necessary to - * start optimization. This system state is the "RotValues" structure, - * which is similar in structure to a STL map, in that it maps - * keys (the label created in step 1) to specific values. - * - * The initial estimate provided to optimization will be used as - * a linearization point for optimization, so it is important that - * all of the variables in the graph have a corresponding value in - * this structure. - * - * The interface to all RotValues types is the same, it only depends - * on the type of key used to find the appropriate value map if there - * are multiple types of variables. - */ - Values initial; - initial.insert(key, Rot2::fromAngle(20 * degree)); - initial.print("initial estimate"); + /** + * Step 3: Create an initial estimate + * An initial estimate of the solution for the system is necessary to + * start optimization. This system state is the "RotValues" structure, + * which is similar in structure to a STL map, in that it maps + * keys (the label created in step 1) to specific values. + * + * The initial estimate provided to optimization will be used as + * a linearization point for optimization, so it is important that + * all of the variables in the graph have a corresponding value in + * this structure. + * + * The interface to all RotValues types is the same, it only depends + * on the type of key used to find the appropriate value map if there + * are multiple types of variables. + */ + Values initial; + initial.insert(key, Rot2::fromAngle(20 * degree)); + initial.print("initial estimate"); - /** - * Step 4: Optimize - * After formulating the problem with a graph of constraints - * and an initial estimate, executing optimization is as simple - * as calling a general optimization function with the graph and - * initial estimate. This will yield a new RotValues structure - * with the final state of the optimization. - */ - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("final result"); + /** + * Step 4: Optimize + * After formulating the problem with a graph of constraints + * and an initial estimate, executing optimization is as simple + * as calling a general optimization function with the graph and + * initial estimate. This will yield a new RotValues structure + * with the final state of the optimization. + */ + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("final result"); - return 0; + return 0; } diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 56dbd4726..b029379f2 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -34,23 +34,23 @@ int main(int argc, char** argv) { // Set Number of Nodes in the Graph const int nrNodes = 60; - // Each node takes 1 of 7 possible states denoted by 0-6 in following order: - // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" - // "Industry(with PhD)" "Academia" "Deceased"] - const size_t nrStates = 7; + // Each node takes 1 of 7 possible states denoted by 0-6 in following order: + // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" + // "Industry(with PhD)" "Academia" "Deceased"] + const size_t nrStates = 7; - // define variables + // define variables vector nodes; for (int i = 0; i < nrNodes; i++){ DiscreteKey dk(i, nrStates); nodes.push_back(dk); } - // create graph - DiscreteFactorGraph graph; + // create graph + DiscreteFactorGraph graph; - // add node potentials - graph.add(nodes[0], ".3 .6 .1 0 0 0 0"); + // add node potentials + graph.add(nodes[0], ".3 .6 .1 0 0 0 0"); for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1"); @@ -62,53 +62,53 @@ int main(int argc, char** argv) { "0 0 0 .01 .01 .97 .01 " "0 0 0 0 0 0 1"; - // add edge potentials - for (int i = 0; i < nrNodes - 1; i++) - graph.add(nodes[i] & nodes[i + 1], edgePotential); + // add edge potentials + for (int i = 0; i < nrNodes - 1; i++) + graph.add(nodes[i] & nodes[i + 1], edgePotential); - cout << "Created Factor Graph with " << nrNodes << " variable nodes and " - << graph.size() << " factors (Unary+Edge)."; + cout << "Created Factor Graph with " << nrNodes << " variable nodes and " + << graph.size() << " factors (Unary+Edge)."; - // "Decoding", i.e., configuration with largest value - // We use sequential variable elimination - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); + // "Decoding", i.e., configuration with largest value + // We use sequential variable elimination + DiscreteSequentialSolver solver(graph); + DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); - // "Inference" Computing marginals for each node + // "Inference" Computing marginals for each node - cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl; - tic_(1, "Sequential"); - for (vector::iterator itr = nodes.begin(); itr != nodes.end(); - ++itr) { - //Compute the marginal - Vector margProbs = solver.marginalProbabilities(*itr); + cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl; + tic_(1, "Sequential"); + for (vector::iterator itr = nodes.begin(); itr != nodes.end(); + ++itr) { + //Compute the marginal + Vector margProbs = solver.marginalProbabilities(*itr); - //Print the marginals - cout << "Node#" << setw(4) << itr->first << " : "; - print(margProbs); - } - toc_(1, "Sequential"); + //Print the marginals + cout << "Node#" << setw(4) << itr->first << " : "; + print(margProbs); + } + toc_(1, "Sequential"); - // Here we'll make use of DiscreteMarginals class, which makes use of - // bayes-tree based shortcut evaluation of marginals - DiscreteMarginals marginals(graph); + // Here we'll make use of DiscreteMarginals class, which makes use of + // bayes-tree based shortcut evaluation of marginals + DiscreteMarginals marginals(graph); - cout << "\nComputing Node Marginals ..(BayesTree based)" << endl; - tic_(2, "Multifrontal"); - for (vector::iterator itr = nodes.begin(); itr != nodes.end(); - ++itr) { - //Compute the marginal - Vector margProbs = marginals.marginalProbabilities(*itr); + cout << "\nComputing Node Marginals ..(BayesTree based)" << endl; + tic_(2, "Multifrontal"); + for (vector::iterator itr = nodes.begin(); itr != nodes.end(); + ++itr) { + //Compute the marginal + Vector margProbs = marginals.marginalProbabilities(*itr); - //Print the marginals - cout << "Node#" << setw(4) << itr->first << " : "; - print(margProbs); - } - toc_(2, "Multifrontal"); + //Print the marginals + cout << "Node#" << setw(4) << itr->first << " : "; + print(margProbs); + } + toc_(2, "Multifrontal"); - tictoc_print_(); - return 0; + tictoc_print_(); + return 0; } diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index e9424d369..43cd9b34c 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -25,62 +25,62 @@ using namespace gtsam; int main(int argc, char** argv) { - // We will assume 2-state variables, where, to conform to the "small" example - // we have 0 == "right answer" and 1 == "wrong answer" - size_t nrStates = 2; + // We will assume 2-state variables, where, to conform to the "small" example + // we have 0 == "right answer" and 1 == "wrong answer" + size_t nrStates = 2; - // define variables - DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates), - Allison(4, nrStates); + // define variables + DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates), + Allison(4, nrStates); - // create graph - DiscreteFactorGraph graph; + // create graph + DiscreteFactorGraph graph; - // add node potentials - graph.add(Cathy, "1 3"); - graph.add(Heather, "9 1"); - graph.add(Mark, "1 3"); - graph.add(Allison, "9 1"); + // add node potentials + graph.add(Cathy, "1 3"); + graph.add(Heather, "9 1"); + graph.add(Mark, "1 3"); + graph.add(Allison, "9 1"); - // add edge potentials - graph.add(Cathy & Heather, "2 1 1 2"); - graph.add(Heather & Mark, "2 1 1 2"); - graph.add(Mark & Allison, "2 1 1 2"); + // add edge potentials + graph.add(Cathy & Heather, "2 1 1 2"); + graph.add(Heather & Mark, "2 1 1 2"); + graph.add(Mark & Allison, "2 1 1 2"); - // Print the UGM distribution - cout << "\nUGM distribution:" << endl; - vector allPosbValues = cartesianProduct( - Cathy & Heather & Mark & Allison); - for (size_t i = 0; i < allPosbValues.size(); ++i) { - DiscreteFactor::Values values = allPosbValues[i]; - double prodPot = graph(values); - cout << values[Cathy.first] << " " << values[Heather.first] << " " - << values[Mark.first] << " " << values[Allison.first] << " :\t" - << prodPot << "\t" << prodPot / 3790 << endl; - } + // Print the UGM distribution + cout << "\nUGM distribution:" << endl; + vector allPosbValues = cartesianProduct( + Cathy & Heather & Mark & Allison); + for (size_t i = 0; i < allPosbValues.size(); ++i) { + DiscreteFactor::Values values = allPosbValues[i]; + double prodPot = graph(values); + cout << values[Cathy.first] << " " << values[Heather.first] << " " + << values[Mark.first] << " " << values[Allison.first] << " :\t" + << prodPot << "\t" << prodPot / 3790 << endl; + } - // "Decoding", i.e., configuration with largest value (MPE) - // We use sequential variable elimination - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - optimalDecoding->print("\noptimalDecoding"); + // "Decoding", i.e., configuration with largest value (MPE) + // We use sequential variable elimination + DiscreteSequentialSolver solver(graph); + DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + optimalDecoding->print("\noptimalDecoding"); - // "Inference" Computing marginals - cout << "\nComputing Node Marginals .." << endl; - Vector margProbs; + // "Inference" Computing marginals + cout << "\nComputing Node Marginals .." << endl; + Vector margProbs; - margProbs = solver.marginalProbabilities(Cathy); - print(margProbs, "Cathy's Node Marginal:"); + margProbs = solver.marginalProbabilities(Cathy); + print(margProbs, "Cathy's Node Marginal:"); - margProbs = solver.marginalProbabilities(Heather); - print(margProbs, "Heather's Node Marginal"); + margProbs = solver.marginalProbabilities(Heather); + print(margProbs, "Heather's Node Marginal"); - margProbs = solver.marginalProbabilities(Mark); - print(margProbs, "Mark's Node Marginal"); + margProbs = solver.marginalProbabilities(Mark); + print(margProbs, "Mark's Node Marginal"); - margProbs = solver.marginalProbabilities(Allison); - print(margProbs, "Allison's Node Marginal"); + margProbs = solver.marginalProbabilities(Allison); + print(margProbs, "Allison's Node Marginal"); - return 0; + return 0; } diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 0fc4db330..3c18b220a 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -35,7 +35,7 @@ using namespace gtsam; int main() { - // [code below basically does SRIF with Cholesky] + // [code below basically does SRIF with Cholesky] // Create a factor graph to perform the inference GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph); @@ -46,11 +46,11 @@ int main() { // Create a structure to hold the linearization points Values linearizationPoints; - // Ground truth example - // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 - // Motion model is just moving to the right (x'-x)^2 - // Measurements are GPS like, (x-z)^2, where z is a 2D measurement - // i.e., we should get 0,0 0,1 0,2 if there is no noise + // Ground truth example + // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 + // Motion model is just moving to the right (x'-x)^2 + // Measurements are GPS like, (x-z)^2, where z is a 2D measurement + // i.e., we should get 0,0 0,1 0,2 if there is no noise // Create new state variable Symbol x0('x',0); diff --git a/gtsam.h b/gtsam.h index 130c5260c..f67cb6303 100644 --- a/gtsam.h +++ b/gtsam.h @@ -6,7 +6,7 @@ * * Requirements: * Classes must start with an uppercase letter - * - Can wrap a typedef + * - Can wrap a typedef * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines * Methods can return * - Eigen types: Matrix, Vector @@ -27,20 +27,20 @@ * - The first letter will be made uppercase in the generated MATLAB interface * - Overloads are supported * Arguments to functions any of - * - Eigen types: Matrix, Vector - * - Eigen types and classes as an optionally const reference + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char * - Any class with which be copied with boost::make_shared() (except Eigen) * - boost::shared_ptr of any object type (except Eigen) * Comments can use either C++ or C style, with multiple lines * Namespace definitions * - Names of namespaces must start with a lowercase letter - * - start a namespace with "namespace {" - * - end a namespace with exactly "}" - * - Namespaces can be nested + * - start a namespace with "namespace {" + * - end a namespace with exactly "}" + * - Namespaces can be nested * Namespace usage - * - Namespaces can be specified for classes in arguments and return values - * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" * Includes in C++ wrappers * - All includes will be collected and added in a single file * - All namespaces must have angle brackets: @@ -61,7 +61,7 @@ * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead * of using the copy constructor (which is used for non-virtual objects). * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree - * virtual boost::shared_ptr clone() const; + * virtual boost::shared_ptr clone() const; * Templates * - Basic templates are supported either with an explicit list of types to instantiate, * e.g. template class Class1 { ... }; @@ -124,100 +124,100 @@ namespace gtsam { bool linear_independent(Matrix A, Matrix B, double tol); virtual class Value { - // No constructors because this is an abstract class + // No constructors because this is an abstract class - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Manifold - size_t dim() const; + // Manifold + size_t dim() const; }; #include virtual class LieScalar : gtsam::Value { - // Standard constructors - LieScalar(); - LieScalar(double d); + // Standard constructors + LieScalar(); + LieScalar(double d); - // Standard interface - double value() const; + // Standard interface + double value() const; - // Testable - void print(string s) const; - bool equals(const gtsam::LieScalar& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::LieScalar& expected, double tol) const; - // Group - static gtsam::LieScalar identity(); - gtsam::LieScalar inverse() const; - gtsam::LieScalar compose(const gtsam::LieScalar& p) const; - gtsam::LieScalar between(const gtsam::LieScalar& l2) const; + // Group + static gtsam::LieScalar identity(); + gtsam::LieScalar inverse() const; + gtsam::LieScalar compose(const gtsam::LieScalar& p) const; + gtsam::LieScalar between(const gtsam::LieScalar& l2) const; - // Manifold - size_t dim() const; - gtsam::LieScalar retract(Vector v) const; - Vector localCoordinates(const gtsam::LieScalar& t2) const; + // Manifold + size_t dim() const; + gtsam::LieScalar retract(Vector v) const; + Vector localCoordinates(const gtsam::LieScalar& t2) const; - // Lie group - static gtsam::LieScalar Expmap(Vector v); - static Vector Logmap(const gtsam::LieScalar& p); + // Lie group + static gtsam::LieScalar Expmap(Vector v); + static Vector Logmap(const gtsam::LieScalar& p); }; #include virtual class LieVector : gtsam::Value { - // Standard constructors - LieVector(); - LieVector(Vector v); + // Standard constructors + LieVector(); + LieVector(Vector v); - // Standard interface - Vector vector() const; + // Standard interface + Vector vector() const; - // Testable - void print(string s) const; - bool equals(const gtsam::LieVector& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::LieVector& expected, double tol) const; - // Group - static gtsam::LieVector identity(); - gtsam::LieVector inverse() const; - gtsam::LieVector compose(const gtsam::LieVector& p) const; - gtsam::LieVector between(const gtsam::LieVector& l2) const; + // Group + static gtsam::LieVector identity(); + gtsam::LieVector inverse() const; + gtsam::LieVector compose(const gtsam::LieVector& p) const; + gtsam::LieVector between(const gtsam::LieVector& l2) const; - // Manifold - size_t dim() const; - gtsam::LieVector retract(Vector v) const; - Vector localCoordinates(const gtsam::LieVector& t2) const; + // Manifold + size_t dim() const; + gtsam::LieVector retract(Vector v) const; + Vector localCoordinates(const gtsam::LieVector& t2) const; - // Lie group - static gtsam::LieVector Expmap(Vector v); - static Vector Logmap(const gtsam::LieVector& p); + // Lie group + static gtsam::LieVector Expmap(Vector v); + static Vector Logmap(const gtsam::LieVector& p); }; #include virtual class LieMatrix : gtsam::Value { - // Standard constructors - LieMatrix(); - LieMatrix(Matrix v); + // Standard constructors + LieMatrix(); + LieMatrix(Matrix v); - // Standard interface - Matrix matrix() const; + // Standard interface + Matrix matrix() const; - // Testable - void print(string s) const; - bool equals(const gtsam::LieMatrix& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::LieMatrix& expected, double tol) const; - // Group - static gtsam::LieMatrix identity(); - gtsam::LieMatrix inverse() const; - gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; - gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; + // Group + static gtsam::LieMatrix identity(); + gtsam::LieMatrix inverse() const; + gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; + gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; - // Manifold - size_t dim() const; - gtsam::LieMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::LieMatrix & t2) const; + // Manifold + size_t dim() const; + gtsam::LieMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::LieMatrix & t2) const; - // Lie group - static gtsam::LieMatrix Expmap(Vector v); - static Vector Logmap(const gtsam::LieMatrix& p); + // Lie group + static gtsam::LieMatrix Expmap(Vector v); + static Vector Logmap(const gtsam::LieMatrix& p); }; //************************************************************************* @@ -226,9 +226,9 @@ virtual class LieMatrix : gtsam::Value { virtual class Point2 : gtsam::Value { // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); + Point2(); + Point2(double x, double y); + Point2(Vector v); // Testable void print(string s) const; @@ -288,9 +288,9 @@ virtual class StereoPoint2 : gtsam::Value { virtual class Point3 : gtsam::Value { // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); // Testable void print(string s) const; @@ -310,19 +310,19 @@ virtual class Point3 : gtsam::Value { // Lie Group static gtsam::Point3 Expmap(Vector v); - static Vector Logmap(const gtsam::Point3& p); + static Vector Logmap(const gtsam::Point3& p); // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; + Vector vector() const; + double x() const; + double y() const; + double z() const; }; virtual class Rot2 : gtsam::Value { // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); + Rot2(); + Rot2(double theta); static gtsam::Rot2 fromAngle(double theta); static gtsam::Rot2 fromDegrees(double theta); static gtsam::Rot2 fromCosSin(double c, double s); @@ -345,47 +345,47 @@ virtual class Rot2 : gtsam::Value { // Lie Group static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); + static Vector Logmap(const gtsam::Rot2& p); // Group Action on Point2 gtsam::Point2 rotate(const gtsam::Point2& point) const; gtsam::Point2 unrotate(const gtsam::Point2& point) const; // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; Matrix matrix() const; }; virtual class Rot3 : gtsam::Value { // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); + Rot3(); + Rot3(Matrix R); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); static gtsam::Rot3 RzRyRx(double x, double y, double z); // FIXME: overloaded functions don't work yet - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 ypr(double y, double p, double r); - static gtsam::Rot3 quaternion(double w, double x, double y, double z); - static gtsam::Rot3 rodriguez(Vector v); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 ypr(double y, double p, double r); + static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 rodriguez(Vector v); // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; // Group - static gtsam::Rot3 identity(); + static gtsam::Rot3 identity(); gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; // Manifold static size_t Dim(); @@ -395,31 +395,31 @@ virtual class Rot3 : gtsam::Value { Vector localCoordinates(const gtsam::Rot3& p) const; // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly }; virtual class Pose2 : gtsam::Value { // Standard Constructor - Pose2(); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); + Pose2(); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); // Testable void print(string s) const; @@ -438,8 +438,8 @@ virtual class Pose2 : gtsam::Value { Vector localCoordinates(const gtsam::Pose2& p) const; // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); Matrix adjointMap() const; Vector adjoint(const Vector& xi) const; static Matrix wedge(double vx, double vy, double w); @@ -449,70 +449,70 @@ virtual class Pose2 : gtsam::Value { gtsam::Point2 transform_to(const gtsam::Point2& p) const; // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; Matrix matrix() const; }; virtual class Pose3 : gtsam::Value { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& pose); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& pose); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) + Pose3(Matrix t); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& p2) const; + gtsam::Pose3 between(const gtsam::Pose3& p2) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Pose3 retract(Vector v) const; - gtsam::Pose3 retractFirstOrder(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Pose3 retract(Vector v) const; + gtsam::Pose3 retractFirstOrder(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& T2) const; - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); - Matrix adjointMap() const; - Vector adjoint(const Vector& xi) const; - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& p); + Matrix adjointMap() const; + Vector adjoint(const Vector& xi) const; + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; + // Group Action on Point3 + gtsam::Point3 transform_from(const gtsam::Point3& p) const; + gtsam::Point3 transform_to(const gtsam::Point3& p) const; - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); }; virtual class Cal3_S2 : gtsam::Value { // Standard Constructors Cal3_S2(); Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); // Testable void print(string s) const; @@ -542,34 +542,34 @@ virtual class Cal3_S2 : gtsam::Value { #include virtual class Cal3DS2 : gtsam::Value { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3DS2(Vector v); + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); + Cal3DS2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // TODO: D2d functions that start with an uppercase letter + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // TODO: D2d functions that start with an uppercase letter - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase }; class Cal3_S2Stereo { @@ -593,14 +593,14 @@ class Cal3_S2Stereo { virtual class CalibratedCamera : gtsam::Value { // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(const Vector& v); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold static size_t Dim(); @@ -617,13 +617,13 @@ virtual class CalibratedCamera : gtsam::Value { static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); // Standard Interface - gtsam::Pose3 pose() const; + gtsam::Pose3 pose() const; double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods }; virtual class SimpleCamera : gtsam::Value { // Standard Constructors and Named Constructors - SimpleCamera(); + SimpleCamera(); SimpleCamera(const gtsam::Pose3& pose); SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, @@ -634,14 +634,14 @@ virtual class SimpleCamera : gtsam::Value { const gtsam::Cal3_S2& K); // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; // Standard Interface gtsam::Pose3 pose() const; gtsam::Cal3_S2 calibration(); - // Manifold + // Manifold gtsam::SimpleCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::SimpleCamera& T2) const; size_t dim() const; @@ -650,8 +650,8 @@ virtual class SimpleCamera : gtsam::Value { // Transformations and measurement functions static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& point); // FIXME, overload }; @@ -659,38 +659,38 @@ virtual class SimpleCamera : gtsam::Value { // TODO: Add this back in when Cal3DS2 has a calibrate function //template //virtual class PinholeCamera : gtsam::Value { -// // Standard Constructors and Named Constructors -// PinholeCamera(); -// PinholeCamera(const gtsam::Pose3& pose); -// PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); -// static This Level(const gtsam::Cal3DS2& K, -// const gtsam::Pose2& pose, double height); -// static This Level(const gtsam::Pose2& pose, double height); // FIXME overload -// static This Lookat(const gtsam::Point3& eye, -// const gtsam::Point3& target, const gtsam::Point3& upVector, -// const gtsam::Cal3DS2& K); +// // Standard Constructors and Named Constructors +// PinholeCamera(); +// PinholeCamera(const gtsam::Pose3& pose); +// PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); +// static This Level(const gtsam::Cal3DS2& K, +// const gtsam::Pose2& pose, double height); +// static This Level(const gtsam::Pose2& pose, double height); // FIXME overload +// static This Lookat(const gtsam::Point3& eye, +// const gtsam::Point3& target, const gtsam::Point3& upVector, +// const gtsam::Cal3DS2& K); // -// // Testable -// void print(string s) const; -// bool equals(const This& camera, double tol) const; +// // Testable +// void print(string s) const; +// bool equals(const This& camera, double tol) const; // -// // Standard Interface -// gtsam::Pose3 pose() const; -// CALIBRATION calibration() const; +// // Standard Interface +// gtsam::Pose3 pose() const; +// CALIBRATION calibration() const; // -// // Manifold -// This retract(const Vector& d) const; -// Vector localCoordinates(const This& T2) const; -// size_t dim() const; -// static size_t Dim(); +// // Manifold +// This retract(const Vector& d) const; +// Vector localCoordinates(const This& T2) const; +// size_t dim() const; +// static size_t Dim(); // -// // Transformations and measurement functions -// static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); -// pair projectSafe(const gtsam::Point3& pw) const; -// gtsam::Point2 project(const gtsam::Point3& point); -// gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; -// double range(const gtsam::Point3& point); -// double range(const gtsam::Pose3& point); // FIXME, overload +// // Transformations and measurement functions +// static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); +// pair projectSafe(const gtsam::Point3& pw) const; +// gtsam::Point2 project(const gtsam::Point3& point); +// gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; +// double range(const gtsam::Point3& point); +// double range(const gtsam::Pose3& point); // FIXME, overload //}; //************************************************************************* @@ -698,25 +698,25 @@ virtual class SimpleCamera : gtsam::Value { //************************************************************************* #include class Permutation { - // Standard Constructors and Named Constructors - Permutation(); - Permutation(size_t nVars); - static gtsam::Permutation Identity(size_t nVars); + // Standard Constructors and Named Constructors + Permutation(); + Permutation(size_t nVars); + static gtsam::Permutation Identity(size_t nVars); - // Testable - void print(string s) const; - bool equals(const gtsam::Permutation& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Permutation& rhs, double tol) const; - // Standard interface - size_t at(size_t variable) const; - size_t size() const; - bool empty() const; - void resize(size_t newSize); - gtsam::Permutation* permute(const gtsam::Permutation& permutation) const; - gtsam::Permutation* inverse() const; - // FIXME: Cannot currently wrap std::vector - //static gtsam::Permutation PullToFront(const vector& toFront, size_t size, bool filterDuplicates); - //static gtsam::Permutation PushToBack(const vector& toBack, size_t size, bool filterDuplicates = false); + // Standard interface + size_t at(size_t variable) const; + size_t size() const; + bool empty() const; + void resize(size_t newSize); + gtsam::Permutation* permute(const gtsam::Permutation& permutation) const; + gtsam::Permutation* inverse() const; + // FIXME: Cannot currently wrap std::vector + //static gtsam::Permutation PullToFront(const vector& toFront, size_t size, bool filterDuplicates); + //static gtsam::Permutation PushToBack(const vector& toBack, size_t size, bool filterDuplicates = false); gtsam::Permutation* partialPermutation(const gtsam::Permutation& selector, const gtsam::Permutation& partialPermutation) const; }; @@ -772,23 +772,23 @@ class IndexConditional { #include template virtual class BayesNet { - // Testable - void print(string s) const; - bool equals(const This& other, double tol) const; + // Testable + void print(string s) const; + bool equals(const This& other, double tol) const; - // Standard interface - size_t size() const; - void printStats(string s) const; + // Standard interface + size_t size() const; + void printStats(string s) const; void saveGraph(string s) const; - CONDITIONAL* front() const; - CONDITIONAL* back() const; - void push_back(CONDITIONAL* conditional); - void push_front(CONDITIONAL* conditional); - void push_back(This& conditional); - void push_front(This& conditional); - void pop_front(); - void permuteWithInverse(const gtsam::Permutation& inversePermutation); - bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); + CONDITIONAL* front() const; + CONDITIONAL* back() const; + void push_back(CONDITIONAL* conditional); + void push_front(CONDITIONAL* conditional); + void push_back(This& conditional); + void push_front(This& conditional); + void pop_front(); + void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; #include @@ -803,15 +803,15 @@ virtual class BayesTree { bool equals(const This& other, double tol) const; //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; + //size_t findParentClique(const gtsam::IndexVector& parents) const; size_t size(); void saveGraph(string s) const; CLIQUE* root() const; void clear(); void deleteCachedShorcuts(); void insert(const CLIQUE* subtree); - size_t numCachedShortcuts() const; - size_t numCachedSeparatorMarginals() const; + size_t numCachedShortcuts() const; + size_t numCachedSeparatorMarginals() const; }; template @@ -824,8 +824,8 @@ virtual class BayesTreeClique { void print(string s) const; void printTree() const; // Default indent of "" void printTree(string indent) const; - size_t numCachedShortcuts() const; - size_t numCachedSeparatorMarginals() const; + size_t numCachedShortcuts() const; + size_t numCachedSeparatorMarginals() const; CONDITIONAL* conditional() const; bool isRoot() const; @@ -972,19 +972,19 @@ virtual class Base { }; virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - //Matrix R() const; // FIXME: cannot parse!!! - bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + //Matrix R() const; // FIXME: cannot parse!!! + bool equals(gtsam::noiseModel::Base& expected, double tol); + void print(string s) const; }; virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); -// Matrix R() const; // FIXME: cannot parse!!! - void print(string s) const; + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); +// Matrix R() const; // FIXME: cannot parse!!! + void print(string s) const; }; virtual class Constrained : gtsam::noiseModel::Diagonal { @@ -1002,83 +1002,83 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { }; virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + void print(string s) const; }; virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; + static gtsam::noiseModel::Unit* Create(size_t dim); + void print(string s) const; }; }///\namespace noiseModel #include class Sampler { //Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - Sampler(int seed); + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + Sampler(int seed); //Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); + Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; class VectorValues { //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - VectorValues(size_t nVars, size_t varDim); + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + VectorValues(size_t nVars, size_t varDim); - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - static gtsam::VectorValues Zero(size_t nVars, size_t varDim); - static gtsam::VectorValues SameStructure(const gtsam::VectorValues& other); + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + static gtsam::VectorValues Zero(size_t nVars, size_t varDim); + static gtsam::VectorValues SameStructure(const gtsam::VectorValues& other); - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - size_t dim() const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + size_t dim() const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; - //Advanced Interface - void resizeLike(const gtsam::VectorValues& other); - void resize(size_t nVars, size_t varDim); - void setZero(); + //Advanced Interface + void resizeLike(const gtsam::VectorValues& other); + void resize(size_t nVars, size_t varDim); + void setZero(); //FIXME: Parse errors with vector() - //const Vector& vector() const; - //Vector& vector(); - bool hasSameStructure(const gtsam::VectorValues& other) const; + //const Vector& vector() const; + //Vector& vector(); + bool hasSameStructure(const gtsam::VectorValues& other) const; double dot(const gtsam::VectorValues& V) const; double norm() const; }; class GaussianConditional { //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - Vector sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, Vector sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + Vector sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, Vector sigmas); - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - size_t dim() const; + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + size_t dim() const; - //Advanced Interface - Matrix computeInformation() const; + //Advanced Interface + Matrix computeInformation() const; gtsam::JacobianFactor* toFactor() const; void solveInPlace(gtsam::VectorValues& x) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; @@ -1087,20 +1087,20 @@ class GaussianConditional { class GaussianDensity { //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas); + GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas); - //Standard Interface - void print(string s) const; - Vector mean() const; - Matrix information() const; - Matrix covariance() const; + //Standard Interface + void print(string s) const; + Vector mean() const; + Matrix information() const; + Matrix covariance() const; }; typedef gtsam::BayesNet GaussianBayesNetBase; virtual class GaussianBayesNet : gtsam::GaussianBayesNetBase { //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); }; //Non-Class methods found in GaussianBayesNet.h @@ -1126,41 +1126,41 @@ typedef gtsam::BayesTreeClique GaussianBayesTreeCliq typedef gtsam::BayesTree GaussianBayesTreeBase; virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase { // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesNet& bn); - GaussianBayesTree(const gtsam::GaussianBayesNet& other); + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesNet& bn); + GaussianBayesTree(const gtsam::GaussianBayesNet& other); }; virtual class GaussianFactor { - void print(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* negate() const; - size_t size() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* negate() const; + size_t size() const; }; virtual class JacobianFactor : gtsam::GaussianFactor { //Constructors - JacobianFactor(); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactor& factor); - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; - //Standard Interface + //Standard Interface gtsam::GaussianFactor* negate() const; bool empty() const; Matrix getA() const; @@ -1190,81 +1190,81 @@ virtual class JacobianFactor : gtsam::GaussianFactor { virtual class HessianFactor : gtsam::GaussianFactor { //Constructors - HessianFactor(const gtsam::HessianFactor& gf); - HessianFactor(); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianConditional& cg); - HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(const gtsam::HessianFactor& gf); + HessianFactor(); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianConditional& cg); + HessianFactor(const gtsam::GaussianFactor& factor); - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; - //Standard Interface - size_t rows() const; - gtsam::GaussianFactor* negate() const; - Matrix info() const; - double constantTerm() const; - Vector linearTerm() const; - Matrix computeInformation() const; + //Standard Interface + size_t rows() const; + gtsam::GaussianFactor* negate() const; + Matrix info() const; + double constantTerm() const; + Vector linearTerm() const; + Matrix computeInformation() const; - //Advanced Interface - void partialCholesky(size_t nrFrontals); + //Advanced Interface + void partialCholesky(size_t nrFrontals); gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals); void assertInvariants() const; }; class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN); + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN); - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; - // Inference - pair eliminateFrontals(size_t nFrontals) const; + // Inference + pair eliminateFrontals(size_t nFrontals) const; - // Building the graph - void push_back(gtsam::GaussianFactor* factor); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); + // Building the graph + void push_back(gtsam::GaussianFactor* factor); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); //Permutations - void permuteWithInverse(const gtsam::Permutation& inversePermutation); + void permuteWithInverse(const gtsam::Permutation& inversePermutation); - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; - // combining - static gtsam::GaussianFactorGraph combine2( - const gtsam::GaussianFactorGraph& lfg1, - const gtsam::GaussianFactorGraph& lfg2); - void combine(const gtsam::GaussianFactorGraph& lfg); + // combining + static gtsam::GaussianFactorGraph combine2( + const gtsam::GaussianFactorGraph& lfg1, + const gtsam::GaussianFactorGraph& lfg2); + void combine(const gtsam::GaussianFactorGraph& lfg); - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - Matrix augmentedHessian() const; - pair hessian() const; + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + Matrix augmentedHessian() const; + pair hessian() const; }; //Non-Class functions in GaussianFactorGraph.h @@ -1304,15 +1304,15 @@ virtual class GaussianISAM : gtsam::GaussianBayesTree { #include class GaussianSequentialSolver { //Constructors - GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, - bool useQR); + GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph, + bool useQR); - //Standard Interface - void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph); - gtsam::GaussianBayesNet* eliminate() const; - gtsam::VectorValues* optimize() const; - gtsam::GaussianFactor* marginalFactor(size_t j) const; - Matrix marginalCovariance(size_t j) const; + //Standard Interface + void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph); + gtsam::GaussianBayesNet* eliminate() const; + gtsam::VectorValues* optimize() const; + gtsam::GaussianFactor* marginalFactor(size_t j) const; + Matrix marginalCovariance(size_t j) const; }; #include @@ -1360,21 +1360,21 @@ class SubgraphSolver { #include class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); }; //************************************************************************* @@ -1391,12 +1391,12 @@ class Ordering { // Standard Constructors and Named Constructors Ordering(); - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering& ord, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering& ord, double tol) const; - // Standard interface - size_t nVars() const; + // Standard interface + size_t nVars() const; size_t size() const; size_t at(size_t key) const; bool exists(size_t key) const; @@ -1407,80 +1407,80 @@ class Ordering { }; class InvertedOrdering { - InvertedOrdering(); + InvertedOrdering(); - // FIXME: add bracket operator overload + // FIXME: add bracket operator overload - bool empty() const; - size_t size() const; - bool count(size_t index) const; // Use as a boolean function with implicit cast + bool empty() const; + size_t size() const; + bool count(size_t index) const; // Use as a boolean function with implicit cast - void clear(); + void clear(); }; class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t i) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); - // NonlinearFactorGraph - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - void add(const gtsam::NonlinearFactor* factor); - gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values, - const gtsam::Ordering& ordering) const; - gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const; - gtsam::NonlinearFactorGraph clone() const; + // NonlinearFactorGraph + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + void add(const gtsam::NonlinearFactor* factor); + gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values, + const gtsam::Ordering& ordering) const; + gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const; + gtsam::NonlinearFactorGraph clone() const; }; virtual class NonlinearFactor { - void print(string s) const; - void printKeys(string s) const; - void equals(const gtsam::NonlinearFactor& other, double tol) const; - gtsam::KeyVector keys() const; - size_t size() const; - size_t dim() const; - double error(const gtsam::Values& c) const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; - gtsam::NonlinearFactor* clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen + void print(string s) const; + void printKeys(string s) const; + void equals(const gtsam::NonlinearFactor& other, double tol) const; + gtsam::KeyVector keys() const; + size_t size() const; + size_t dim() const; + double error(const gtsam::Values& c) const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen }; #include class Values { - Values(); - Values(const gtsam::Values& other); + Values(); + Values(const gtsam::Values& other); - size_t size() const; - bool empty() const; + size_t size() const; + bool empty() const; void clear(); size_t dim() const; - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; - void insert(size_t j, const gtsam::Value& value); + void insert(size_t j, const gtsam::Value& value); void insert(const gtsam::Values& values); void update(size_t j, const gtsam::Value& val); void update(const gtsam::Values& values); void erase(size_t j); void swap(gtsam::Values& values); - bool exists(size_t j) const; - gtsam::Value at(size_t j) const; - gtsam::KeyList keys() const; + bool exists(size_t j) const; + gtsam::Value at(size_t j) const; + gtsam::KeyList keys() const; gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const; gtsam::Ordering* orderingArbitrary(size_t firstVar) const; @@ -1493,86 +1493,86 @@ class Values { // Actually a FastList #include class KeyList { - KeyList(); - KeyList(const gtsam::KeyList& other); + KeyList(); + KeyList(const gtsam::KeyList& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void sort(); - void remove(size_t key); + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void sort(); + void remove(size_t key); }; // Actually a FastSet #include class KeySet { - KeySet(); - KeySet(const gtsam::KeySet& other); - KeySet(const gtsam::KeyVector& other); - KeySet(const gtsam::KeyList& other); + KeySet(); + KeySet(const gtsam::KeySet& other); + KeySet(const gtsam::KeyVector& other); + KeySet(const gtsam::KeyList& other); - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet& other) const; + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(size_t key); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + // structure specific methods + void insert(size_t key); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists }; // Actually a FastVector #include class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - KeyVector(const gtsam::KeySet& other); - KeyVector(const gtsam::KeyList& other); + KeyVector(); + KeyVector(const gtsam::KeyVector& other); + KeyVector(const gtsam::KeySet& other); + KeyVector(const gtsam::KeyList& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; }; #include class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; }; class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; }; //************************************************************************* @@ -1581,31 +1581,31 @@ class JointMarginal { #include virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s) const; + NonlinearOptimizerParams(); + void print(string s) const; - size_t getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; + size_t getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; - void setMaxIterations(size_t value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); + void setMaxIterations(size_t value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); }; #include virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { SuccessiveLinearizationParams(); - string getLinearSolverType() const; - - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); + string getLinearSolverType() const; + + void setLinearSolverType(string solver); + void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); bool isMultifrontal() const; bool isSequential() const; @@ -1615,12 +1615,12 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { #include virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { - GaussNewtonParams(); + GaussNewtonParams(); }; #include virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { - LevenbergMarquardtParams(); + LevenbergMarquardtParams(); double getlambdaInitial() const; double getlambdaFactor() const; @@ -1635,39 +1635,39 @@ virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { #include virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { - DoglegParams(); + DoglegParams(); - double getDeltaInitial() const; - string getVerbosityDL() const; + double getDeltaInitial() const; + string getVerbosityDL() const; - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; }; virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - void iterate() const; + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; }; virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); }; virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; }; virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; void print(string str) const; }; @@ -1798,12 +1798,12 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree { #include class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); + NonlinearISAM(); + NonlinearISAM(int reorderInterval); void print(string s) const; void printStats() const; void saveGraph(string s) const; - gtsam::Values estimate() const; + gtsam::Values estimate() const; Matrix marginalCovariance(size_t key) const; int reorderInterval() const; int reorderCounter() const; @@ -1832,31 +1832,31 @@ class NonlinearISAM { #include template virtual class PriorFactor : gtsam::NonlinearFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); }; #include template virtual class BetweenFactor : gtsam::NonlinearFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); }; #include template virtual class NonlinearEquality : gtsam::NonlinearFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); }; #include template virtual class RangeFactor : gtsam::NonlinearFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::RangeFactor RangeFactorPosePoint2; @@ -1872,7 +1872,7 @@ typedef gtsam::RangeFactor RangeFactor #include template virtual class BearingFactor : gtsam::NonlinearFactor { - BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::BearingFactor BearingFactor2D; @@ -1881,7 +1881,7 @@ typedef gtsam::BearingFactor BearingFa #include template virtual class BearingRangeFactor : gtsam::NonlinearFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::BearingRangeFactor BearingRangeFactor2D; @@ -1890,10 +1890,10 @@ typedef gtsam::BearingRangeFactor Bear #include template virtual class GenericProjectionFactor : gtsam::NonlinearFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k); + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; // FIXME: Add Cal3DS2 when it has a 'calibrate' function @@ -1903,8 +1903,8 @@ typedef gtsam::GenericProjectionFactor template virtual class GeneralSFMFactor : gtsam::NonlinearFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; // FIXME: Add Cal3DS2 when it has a 'calibrate' function @@ -1913,18 +1913,18 @@ typedef gtsam::GeneralSFMFactor GeneralSFMFa // FIXME: Add Cal3DS2 when it has a 'calibrate' function template virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; }; #include template virtual class GenericStereoFactor : gtsam::NonlinearFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; }; typedef gtsam::GenericStereoFactor GenericStereoFactor3D; @@ -1932,11 +1932,11 @@ typedef gtsam::GenericStereoFactor GenericStereoFac pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID); + gtsam::noiseModel::Diagonal* model, int maxID); pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); + gtsam::noiseModel::Diagonal* model); //************************************************************************* diff --git a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h index c253ed522..55693d47d 100644 --- a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h +++ b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h @@ -32,15 +32,15 @@ extern "C" { /* All versions of CCOLAMD will include the following definitions. * As an example, to test if the version you are using is 1.3 or later: * - * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... + * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... * * This also works during compile-time: * - * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) - * printf ("This is version 1.3 or later\n") ; - * #else - * printf ("This is an early version\n") ; - * #endif + * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) + * printf ("This is version 1.3 or later\n") ; + * #else + * printf ("This is an early version\n") ; + * #endif */ #define CCOLAMD_DATE "Jan 25, 2011" @@ -49,7 +49,7 @@ extern "C" { #define CCOLAMD_SUB_VERSION 7 #define CCOLAMD_SUBSUB_VERSION 3 #define CCOLAMD_VERSION \ - CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) + CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) /* ========================================================================== */ /* === Knob and statistics definitions ====================================== */ @@ -94,20 +94,20 @@ extern "C" { #define CCOLAMD_NEWLY_EMPTY_COL 10 /* error codes returned in stats [3]: */ -#define CCOLAMD_OK (0) -#define CCOLAMD_OK_BUT_JUMBLED (1) -#define CCOLAMD_ERROR_A_not_present (-1) -#define CCOLAMD_ERROR_p_not_present (-2) -#define CCOLAMD_ERROR_nrow_negative (-3) -#define CCOLAMD_ERROR_ncol_negative (-4) -#define CCOLAMD_ERROR_nnz_negative (-5) -#define CCOLAMD_ERROR_p0_nonzero (-6) -#define CCOLAMD_ERROR_A_too_small (-7) -#define CCOLAMD_ERROR_col_length_negative (-8) -#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) -#define CCOLAMD_ERROR_out_of_memory (-10) -#define CCOLAMD_ERROR_invalid_cmember (-11) -#define CCOLAMD_ERROR_internal_error (-999) +#define CCOLAMD_OK (0) +#define CCOLAMD_OK_BUT_JUMBLED (1) +#define CCOLAMD_ERROR_A_not_present (-1) +#define CCOLAMD_ERROR_p_not_present (-2) +#define CCOLAMD_ERROR_nrow_negative (-3) +#define CCOLAMD_ERROR_ncol_negative (-4) +#define CCOLAMD_ERROR_nnz_negative (-5) +#define CCOLAMD_ERROR_p0_nonzero (-6) +#define CCOLAMD_ERROR_A_too_small (-7) +#define CCOLAMD_ERROR_col_length_negative (-8) +#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) +#define CCOLAMD_ERROR_out_of_memory (-10) +#define CCOLAMD_ERROR_invalid_cmember (-11) +#define CCOLAMD_ERROR_internal_error (-999) /* ========================================================================== */ /* === Prototypes of user-callable routines ================================= */ @@ -116,45 +116,45 @@ extern "C" { /* define UF_long */ #include "UFconfig.h" -size_t ccolamd_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - int nnz, /* nonzeros in A */ - int n_row, /* number of rows in A */ - int n_col /* number of columns in A */ + int nnz, /* nonzeros in A */ + int n_row, /* number of rows in A */ + int n_col /* number of columns in A */ ) ; -size_t ccolamd_l_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_l_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - UF_long nnz, /* nonzeros in A */ - UF_long n_row, /* number of rows in A */ - UF_long n_col /* number of columns in A */ + UF_long nnz, /* nonzeros in A */ + UF_long n_row, /* number of rows in A */ + UF_long n_col /* number of columns in A */ ) ; -void ccolamd_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -void ccolamd_l_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_l_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -int ccolamd /* returns (1) if successful, (0) otherwise*/ -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +int ccolamd /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ - int cmember [ ] /* Constraint set of A, of size n_col */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int cmember [ ] /* Constraint set of A, of size n_col */ ) ; -UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ +UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ ( UF_long n_row, UF_long n_col, @@ -166,23 +166,23 @@ UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ UF_long cmember [ ] ) ; -int csymamd /* return (1) if OK, (0) otherwise */ +int csymamd /* return (1) if OK, (0) otherwise */ ( - int n, /* number of rows and columns of A */ - int A [ ], /* row indices of A */ - int p [ ], /* column pointers of A */ - int perm [ ], /* output permutation, size n_col+1 */ + int n, /* number of rows and columns of A */ + int A [ ], /* row indices of A */ + int p [ ], /* column pointers of A */ + int perm [ ], /* output permutation, size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */ - int stats [CCOLAMD_STATS], /* output statistics and error codes */ + int stats [CCOLAMD_STATS], /* output statistics and error codes */ void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */ - /* mxCalloc (for MATLAB mexFunction) */ - void (*release) (void *), /* pointer to free (ANSI C) or */ - /* mxFree (for MATLAB mexFunction) */ - int cmember [ ], /* Constraint set of A */ - int stype /* 0: use both parts, >0: upper, <0: lower */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *), /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ + int cmember [ ], /* Constraint set of A */ + int stype /* 0: use both parts, >0: upper, <0: lower */ ) ; -UF_long csymamd_l /* same as csymamd, but with UF_long integers */ +UF_long csymamd_l /* same as csymamd, but with UF_long integers */ ( UF_long n, UF_long A [ ], @@ -227,26 +227,26 @@ void csymamd_l_report */ int ccolamd2 -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ /* each Front_ array is of size n_col+1: */ - int Front_npivcol [ ], /* # pivot cols in each front */ - int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ - int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ - int Front_parent [ ], /* parent of each front */ - int Front_cols [ ], /* link list of pivot columns for each front */ - int *p_nfr, /* total number of frontal matrices */ - int InFront [ ], /* InFront [row] = f if row in front f */ - int cmember [ ] /* Constraint set of A */ + int Front_npivcol [ ], /* # pivot cols in each front */ + int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ + int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ + int Front_parent [ ], /* parent of each front */ + int Front_cols [ ], /* link list of pivot columns for each front */ + int *p_nfr, /* total number of frontal matrices */ + int InFront [ ], /* InFront [row] = f if row in front f */ + int cmember [ ] /* Constraint set of A */ ) ; -UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ +UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ ( UF_long n_row, UF_long n_col, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index a5e3d5469..ef257aded 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -514,12 +514,12 @@ struct solve_retval, Rhs> typedef typename LDLTType::RealScalar RealScalar; const Diagonal vectorD = dec().vectorD(); RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits::epsilon(), - RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS + RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS for (Index i = 0; i < vectorD.size(); ++i) { if(abs(vectorD(i)) > tolerance) - dst.row(i) /= vectorD(i); + dst.row(i) /= vectorD(i); else - dst.row(i).setZero(); + dst.row(i).setZero(); } // dst = L^-T (D^-1 L^-1 P b) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h index 91b4fa1e2..327c3f3ea 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h @@ -324,7 +324,7 @@ void ComplexEigenSolver::sortEigenvalues(bool computeEigenvectors) k += i; std::swap(m_eivalues[k],m_eivalues[i]); if(computeEigenvectors) - m_eivec.col(i).swap(m_eivec.col(k)); + m_eivec.col(i).swap(m_eivec.col(k)); } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h index f9365ae59..95d5dcdcb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h @@ -547,7 +547,7 @@ void EigenSolver::doComputeEigenvectors() if ((vr == 0.0) && (vi == 0.0)) vr = eps * norm * (internal::abs(w) + internal::abs(q) + internal::abs(x) + internal::abs(y) + internal::abs(lastw)); - std::complex cc = cdiv(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra,vr,vi); + std::complex cc = cdiv(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra,vr,vi); m_matT.coeffRef(i,n-1) = internal::real(cc); m_matT.coeffRef(i,n) = internal::imag(cc); if (internal::abs(x) > (internal::abs(lastw) + internal::abs(q))) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h index 88e63eba4..4fce56f65 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h @@ -197,8 +197,8 @@ template class HessenbergDecomposition /** \brief Returns the internal representation of the decomposition * - * \returns a const reference to a matrix with the internal representation - * of the decomposition. + * \returns a const reference to a matrix with the internal representation + * of the decomposition. * * \pre Either the constructor HessenbergDecomposition(const MatrixType&) * or the member function compute(const MatrixType&) has been called diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h index a004e7e63..bd0f6d93f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h @@ -141,10 +141,10 @@ MatrixBase::operatorNorm() const // then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough. return internal::sqrt((m_eval*m_eval.adjoint()) .eval() - .template selfadjointView() - .eigenvalues() - .maxCoeff() - ); + .template selfadjointView() + .eigenvalues() + .maxCoeff() + ); } /** \brief Computes the L2 operator norm diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h index e8f0ac5d1..5b03a4a52 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h @@ -200,8 +200,8 @@ template class Tridiagonalization /** \brief Returns the internal representation of the decomposition * - * \returns a const reference to a matrix with the internal representation - * of the decomposition. + * \returns a const reference to a matrix with the internal representation + * of the decomposition. * * \pre Either the constructor Tridiagonalization(const MatrixType&) or * the member function compute(const MatrixType&) has been called before diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 75083363c..99f069801 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -179,7 +179,7 @@ public: bool isApprox(const QuaternionBase& other, RealScalar prec = NumTraits::dummy_precision()) const { return coeffs().isApprox(other.coeffs(), prec); } - /** return the result vector of \a v through the rotation*/ + /** return the result vector of \a v through the rotation*/ EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; /** \returns \c *this with scalar type casted to \a NewScalarType diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 9550b6bf6..a98829ade 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -487,7 +487,7 @@ struct solve_retval, Rhs> // Note that the matrix Q = H_0^* H_1^*... so its inverse is Q^* = (H_0 H_1 ...)^T c.applyOnTheLeft(householderSequence(dec().matrixQR(), dec().hCoeffs()) .setLength(dec().nonzeroPivots()) - .transpose() + .transpose() ); dec().matrixQR() diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h index 6c3eb6858..60c258be4 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h @@ -27,7 +27,7 @@ namespace Eigen { -#define DECL_GSSVX(PREFIX,FLOATTYPE,KEYTYPE) \ +#define DECL_GSSVX(PREFIX,FLOATTYPE,KEYTYPE) \ extern "C" { \ typedef struct { FLOATTYPE for_lu; FLOATTYPE total_needed; int expansions; } PREFIX##mem_usage_t; \ extern void PREFIX##gssvx(superlu_options_t *, SuperMatrix *, int *, int *, int *, \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h b/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h index 6fce99ed5..8fefd80ef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h @@ -159,49 +159,49 @@ int BLASFUNC(qrotm) (int *, double *, int *, double *, int *, double *); /* Level 2 routines */ int BLASFUNC(sger)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(dger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(qger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(cgeru)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(cgerc)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(zgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(zgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(sgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(strsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpsv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpsv) (char *, char *, char *, int *, double *, double *, int *); @@ -211,17 +211,17 @@ int BLASFUNC(ztpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(xtpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(strmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpmv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpmv) (char *, char *, char *, int *, double *, double *, int *); @@ -245,121 +245,121 @@ int BLASFUNC(ztbsv) (char *, char *, char *, int *, int *, double *, int *, doub int BLASFUNC(xtbsv) (char *, char *, char *, int *, int *, double *, int *, double *, int *); int BLASFUNC(ssymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(sspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(csyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ssyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(dsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(qsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(csyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(sspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(dspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(qspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(cspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(zspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(xspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(sspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(dspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(qspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cher) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(chpr) (char *, int *, float *, float *, int *, float *); int BLASFUNC(zhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(xhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(cher2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(chpr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(chemv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chpmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(snorm)(char *, int *, int *, float *, int *); int BLASFUNC(dnorm)(char *, int *, int *, double *, int *); @@ -367,205 +367,205 @@ int BLASFUNC(cnorm)(char *, int *, int *, float *, int *); int BLASFUNC(znorm)(char *, int *, int *, double *, int *); int BLASFUNC(sgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); /* Level 3 routines */ int BLASFUNC(sgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(dgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(qgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm3m)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(sge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(dge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(cge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(zge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(strsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(strmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ssymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(dsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(qsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(csyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(ssyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(qsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(csyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(chemm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chemm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cherk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(cher2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(cher2m)(char *, char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(sgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(cgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(sgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgetf2)(int *, int *, float *, int *, int *, int *); int BLASFUNC(dgetf2)(int *, int *, double *, int *, int *, int *); diff --git a/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp b/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp index 8e4b60c2b..b0950c8f1 100644 --- a/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp +++ b/gtsam/3rdparty/Eigen/bench/benchmarkX.cpp @@ -21,16 +21,16 @@ using namespace Eigen; int main(int argc, char *argv[]) { - MATTYPE I = MATTYPE::Ones(MATSIZE,MATSIZE); - MATTYPE m(MATSIZE,MATSIZE); - for(int i = 0; i < MATSIZE; i++) for(int j = 0; j < MATSIZE; j++) - { - m(i,j) = (i+j+1)/(MATSIZE*MATSIZE); - } - for(int a = 0; a < REPEAT; a++) - { - m = I + 0.0001 * (m + m*m); - } - cout << m(0,0) << endl; - return 0; + MATTYPE I = MATTYPE::Ones(MATSIZE,MATSIZE); + MATTYPE m(MATSIZE,MATSIZE); + for(int i = 0; i < MATSIZE; i++) for(int j = 0; j < MATSIZE; j++) + { + m(i,j) = (i+j+1)/(MATSIZE*MATSIZE); + } + for(int a = 0; a < REPEAT; a++) + { + m = I + 0.0001 * (m + m*m); + } + cout << m(0,0) << endl; + return 0; } diff --git a/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp b/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp index 62437435e..b798d4f51 100644 --- a/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp +++ b/gtsam/3rdparty/Eigen/bench/benchmarkXcwise.cpp @@ -20,16 +20,16 @@ using namespace Eigen; int main(int argc, char *argv[]) { - VECTYPE I = VECTYPE::Ones(VECSIZE); - VECTYPE m(VECSIZE,1); - for(int i = 0; i < VECSIZE; i++) - { - m[i] = 0.1 * i/VECSIZE; - } - for(int a = 0; a < REPEAT; a++) - { - m = VECTYPE::Ones(VECSIZE) + 0.00005 * (m.cwise().square() + m/4); - } - cout << m[0] << endl; - return 0; + VECTYPE I = VECTYPE::Ones(VECSIZE); + VECTYPE m(VECSIZE,1); + for(int i = 0; i < VECSIZE; i++) + { + m[i] = 0.1 * i/VECSIZE; + } + for(int a = 0; a < REPEAT; a++) + { + m = VECTYPE::Ones(VECSIZE) + 0.00005 * (m.cwise().square() + m/4); + } + cout << m[0] << endl; + return 0; } diff --git a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h index d2330d06b..bee46e454 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h +++ b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/utils/utilities.h @@ -25,30 +25,30 @@ /* --- To print date and time of compilation of current source on stdout --- */ # if defined ( __GNUC__ ) -# define COMPILER "g++" ; +# define COMPILER "g++" ; # elif defined ( __sun ) -# define COMPILER "CC" ; +# define COMPILER "CC" ; # elif defined ( __KCC ) -# define COMPILER "KCC" ; +# define COMPILER "KCC" ; # elif defined ( __PGI ) -# define COMPILER "pgCC" ; +# define COMPILER "pgCC" ; # else -# define COMPILER "undefined" ; +# define COMPILER "undefined" ; # endif # ifdef INFOS_COMPILATION # error INFOS_COMPILATION already defined # endif -# define INFOS_COMPILATION {\ - cerr << flush;\ - cout << __FILE__ ;\ - cout << " [" << __LINE__ << "] : " ;\ - cout << "COMPILED with " << COMPILER ;\ - cout << ", " << __DATE__ ; \ - cout << " at " << __TIME__ << endl ;\ - cout << "\n\n" ;\ - cout << flush ;\ - } +# define INFOS_COMPILATION {\ + cerr << flush;\ + cout << __FILE__ ;\ + cout << " [" << __LINE__ << "] : " ;\ + cout << "COMPILED with " << COMPILER ;\ + cout << ", " << __DATE__ ; \ + cout << " at " << __TIME__ << endl ;\ + cout << "\n\n" ;\ + cout << flush ;\ + } # ifdef _DEBUG_ diff --git a/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h b/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h index 28f3a4e57..2ab9c4b9d 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h +++ b/gtsam/3rdparty/Eigen/bench/btl/libs/BLAS/blas.h @@ -180,49 +180,49 @@ int BLASFUNC(qrotm) (int *, double *, int *, double *, int *, double *); /* Level 2 routines */ int BLASFUNC(sger)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(dger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(qger)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(cgeru)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(cgerc)(int *, int *, float *, float *, int *, - float *, int *, float *, int *); + float *, int *, float *, int *); int BLASFUNC(zgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(zgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgeru)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(xgerc)(int *, int *, double *, double *, int *, - double *, int *, double *, int *); + double *, int *, double *, int *); int BLASFUNC(sgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgemv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgemv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(strsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrsv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrsv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpsv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpsv) (char *, char *, char *, int *, double *, double *, int *); @@ -232,17 +232,17 @@ int BLASFUNC(ztpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(xtpsv) (char *, char *, char *, int *, double *, double *, int *); int BLASFUNC(strmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ctrmv) (char *, char *, char *, int *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(ztrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xtrmv) (char *, char *, char *, int *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(stpmv) (char *, char *, char *, int *, float *, float *, int *); int BLASFUNC(dtpmv) (char *, char *, char *, int *, double *, double *, int *); @@ -266,121 +266,121 @@ int BLASFUNC(ztbsv) (char *, char *, char *, int *, int *, double *, int *, doub int BLASFUNC(xtbsv) (char *, char *, char *, int *, int *, double *, int *, double *, int *); int BLASFUNC(ssymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(sspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cspmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xspmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(qsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(csyr) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xsyr) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(ssyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(dsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(qsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(csyr2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xsyr2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(sspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(dspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(qspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(cspr) (char *, int *, float *, float *, int *, - float *); + float *); int BLASFUNC(zspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(xspr) (char *, int *, double *, double *, int *, - double *); + double *); int BLASFUNC(sspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(dspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(qspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cspr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xspr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(cher) (char *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(xher) (char *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(chpr) (char *, int *, float *, float *, int *, float *); int BLASFUNC(zhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(xhpr) (char *, int *, double *, double *, int *, double *); int BLASFUNC(cher2) (char *, int *, float *, - float *, int *, float *, int *, float *, int *); + float *, int *, float *, int *, float *, int *); int BLASFUNC(zher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(xher2) (char *, int *, double *, - double *, int *, double *, int *, double *, int *); + double *, int *, double *, int *, double *, int *); int BLASFUNC(chpr2) (char *, int *, float *, - float *, int *, float *, int *, float *); + float *, int *, float *, int *, float *); int BLASFUNC(zhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(xhpr2) (char *, int *, double *, - double *, int *, double *, int *, double *); + double *, int *, double *, int *, double *); int BLASFUNC(chemv) (char *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemv) (char *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chpmv) (char *, int *, float *, float *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhpmv) (char *, int *, double *, double *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(snorm)(char *, int *, int *, float *, int *); int BLASFUNC(dnorm)(char *, int *, int *, double *, int *); @@ -388,205 +388,205 @@ int BLASFUNC(cnorm)(char *, int *, int *, float *, int *); int BLASFUNC(znorm)(char *, int *, int *, double *, int *); int BLASFUNC(sgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cgbmv)(char *, int *, int *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xgbmv)(char *, int *, int *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chbmv)(char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhbmv)(char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); /* Level 3 routines */ int BLASFUNC(sgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(dgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(qgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(cgemm3m)(char *, char *, int *, int *, int *, float *, - float *, int *, float *, int *, float *, float *, int *); + float *, int *, float *, int *, float *, float *, int *); int BLASFUNC(zgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(xgemm3m)(char *, char *, int *, int *, int *, double *, - double *, int *, double *, int *, double *, double *, int *); + double *, int *, double *, int *, double *, double *, int *); int BLASFUNC(sge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(dge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(cge2mm)(char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *, - float *, float *, int *); + float *, float *, int *, float *, int *, + float *, float *, int *); int BLASFUNC(zge2mm)(char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *, - double *, double *, int *); + double *, double *, int *, double *, int *, + double *, double *, int *); int BLASFUNC(strsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrsm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrsm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(strmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(dtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(qtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ctrmm)(char *, char *, char *, char *, int *, int *, - float *, float *, int *, float *, int *); + float *, float *, int *, float *, int *); int BLASFUNC(ztrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(xtrmm)(char *, char *, char *, char *, int *, int *, - double *, double *, int *, double *, int *); + double *, double *, int *, double *, int *); int BLASFUNC(ssymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(qsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(csymm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xsymm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(ssyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(dsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(qsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(csyrk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xsyrk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(ssyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(dsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(qsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(csyr2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xsyr2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(chemm)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(chemm3m)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(xhemm3m)(char *, char *, int *, int *, double *, double *, int *, - double *, int *, double *, double *, int *); + double *, int *, double *, double *, int *); int BLASFUNC(cherk)(char *, char *, int *, int *, float *, float *, int *, - float *, float *, int *); + float *, float *, int *); int BLASFUNC(zherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(xherk)(char *, char *, int *, int *, double *, double *, int *, - double *, double *, int *); + double *, double *, int *); int BLASFUNC(cher2k)(char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2k)(char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(cher2m)(char *, char *, char *, int *, int *, float *, float *, int *, - float *, int *, float *, float *, int *); + float *, int *, float *, float *, int *); int BLASFUNC(zher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(xher2m)(char *, char *, char *, int *, int *, double *, double *, int *, - double*, int *, double *, double *, int *); + double*, int *, double *, double *, int *); int BLASFUNC(sgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(dgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(cgemt)(char *, int *, int *, float *, float *, int *, - float *, int *); + float *, int *); int BLASFUNC(zgemt)(char *, int *, int *, double *, double *, int *, - double *, int *); + double *, int *); int BLASFUNC(sgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgema)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgema)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(dgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(cgems)(char *, char *, int *, int *, float *, - float *, int *, float *, float *, int *, float *, int *); + float *, int *, float *, float *, int *, float *, int *); int BLASFUNC(zgems)(char *, char *, int *, int *, double *, - double *, int *, double*, double *, int *, double*, int *); + double *, int *, double*, double *, int *, double*, int *); int BLASFUNC(sgetf2)(int *, int *, float *, int *, int *, int *); int BLASFUNC(dgetf2)(int *, int *, double *, int *, int *, int *); diff --git a/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp b/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp index 0623ef0c2..e88f1eb69 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp +++ b/gtsam/3rdparty/Eigen/doc/snippets/TopicStorageOrders_example.cpp @@ -1,7 +1,7 @@ Matrix Acolmajor; Acolmajor << 8, 2, 2, 9, 9, 1, 4, 4, - 3, 5, 4, 5; + 3, 5, 4, 5; cout << "The matrix A:" << endl; cout << Acolmajor << endl << endl; diff --git a/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp index 2adb2e213..aa90c2d1d 100644 --- a/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp +++ b/gtsam/3rdparty/Eigen/doc/snippets/Tutorial_commainit_01b.cpp @@ -1,5 +1,5 @@ Matrix3f m; m.row(0) << 1, 2, 3; m.block(1,0,2,2) << 4, 5, 7, 8; -m.col(2).tail(2) << 6, 9; +m.col(2).tail(2) << 6, 9; std::cout << m; diff --git a/gtsam/3rdparty/Eigen/test/corners.cpp b/gtsam/3rdparty/Eigen/test/corners.cpp index 8d12c6146..d813b6b63 100644 --- a/gtsam/3rdparty/Eigen/test/corners.cpp +++ b/gtsam/3rdparty/Eigen/test/corners.cpp @@ -68,8 +68,8 @@ template void c cols = MatrixType::ColsAtCompileTime, r = CRows, c = CCols, - sr = SRows, - sc = SCols + sr = SRows, + sc = SCols }; VERIFY_IS_EQUAL((matrix.template topLeftCorner()), (matrix.template block(0,0))); diff --git a/gtsam/3rdparty/Eigen/test/eigen2/main.h b/gtsam/3rdparty/Eigen/test/eigen2/main.h index 9d0defa39..6f126e1cf 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/main.h +++ b/gtsam/3rdparty/Eigen/test/eigen2/main.h @@ -114,7 +114,7 @@ namespace Eigen // see bug 89. The copy_bool here is working around a bug in gcc <= 4.3 #define eigen_assert(a) \ - if( (!Eigen::internal::copy_bool(a)) && (!no_more_assert) ) \ + if( (!Eigen::internal::copy_bool(a)) && (!no_more_assert) ) \ { \ Eigen::no_more_assert = true; \ throw Eigen::eigen_assert_exception(); \ @@ -400,7 +400,7 @@ int main(int argc, char *argv[]) if(!has_set_repeat) repeat = DEFAULT_REPEAT; std::cout << "Initializing random number generator with seed " << seed << std::endl; - std::srand(seed); + std::srand(seed); std::cout << "Repeating each test " << repeat << " times" << std::endl; Eigen::g_repeat = repeat; diff --git a/gtsam/3rdparty/Eigen/test/hessenberg.cpp b/gtsam/3rdparty/Eigen/test/hessenberg.cpp index 9d3dc7c84..1e1090f74 100644 --- a/gtsam/3rdparty/Eigen/test/hessenberg.cpp +++ b/gtsam/3rdparty/Eigen/test/hessenberg.cpp @@ -39,7 +39,7 @@ template void hessenberg(int size = Size) VERIFY_IS_APPROX(m, Q * H * Q.adjoint()); for(int row = 2; row < size; ++row) { for(int col = 0; col < row-1; ++col) { - VERIFY(H(row,col) == (typename MatrixType::Scalar)0); + VERIFY(H(row,col) == (typename MatrixType::Scalar)0); } } } diff --git a/gtsam/3rdparty/Eigen/test/schur_complex.cpp b/gtsam/3rdparty/Eigen/test/schur_complex.cpp index 15532e2cc..b2a9d6d6b 100644 --- a/gtsam/3rdparty/Eigen/test/schur_complex.cpp +++ b/gtsam/3rdparty/Eigen/test/schur_complex.cpp @@ -40,7 +40,7 @@ template void schur(int size = MatrixType::ColsAtCompileTim ComplexMatrixType T = schurOfA.matrixT(); for(int row = 1; row < size; ++row) { for(int col = 0; col < row; ++col) { - VERIFY(T(row,col) == (typename MatrixType::Scalar)0); + VERIFY(T(row,col) == (typename MatrixType::Scalar)0); } } VERIFY_IS_APPROX(A.template cast(), U * T * U.adjoint()); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h index e100617d1..c882d87a2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h @@ -69,74 +69,74 @@ namespace internal { */ template bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Preconditioner & precond, - int &iters, const int &restart, typename Dest::RealScalar & tol_error) { + int &iters, const int &restart, typename Dest::RealScalar & tol_error) { - using std::sqrt; - using std::abs; + using std::sqrt; + using std::abs; - typedef typename Dest::RealScalar RealScalar; - typedef typename Dest::Scalar Scalar; - typedef Matrix < RealScalar, Dynamic, 1 > RealVectorType; - typedef Matrix < Scalar, Dynamic, 1 > VectorType; - typedef Matrix < Scalar, Dynamic, Dynamic > FMatrixType; + typedef typename Dest::RealScalar RealScalar; + typedef typename Dest::Scalar Scalar; + typedef Matrix < RealScalar, Dynamic, 1 > RealVectorType; + typedef Matrix < Scalar, Dynamic, 1 > VectorType; + typedef Matrix < Scalar, Dynamic, Dynamic > FMatrixType; - RealScalar tol = tol_error; - const int maxIters = iters; - iters = 0; + RealScalar tol = tol_error; + const int maxIters = iters; + iters = 0; - const int m = mat.rows(); + const int m = mat.rows(); - VectorType p0 = rhs - mat*x; - VectorType r0 = precond.solve(p0); -// RealScalar r0_sqnorm = r0.squaredNorm(); + VectorType p0 = rhs - mat*x; + VectorType r0 = precond.solve(p0); +// RealScalar r0_sqnorm = r0.squaredNorm(); - VectorType w = VectorType::Zero(restart + 1); + VectorType w = VectorType::Zero(restart + 1); - FMatrixType H = FMatrixType::Zero(m, restart + 1); - VectorType tau = VectorType::Zero(restart + 1); - std::vector < JacobiRotation < Scalar > > G(restart); + FMatrixType H = FMatrixType::Zero(m, restart + 1); + VectorType tau = VectorType::Zero(restart + 1); + std::vector < JacobiRotation < Scalar > > G(restart); - // generate first Householder vector - VectorType e; - RealScalar beta; - r0.makeHouseholder(e, tau.coeffRef(0), beta); - w(0)=(Scalar) beta; - H.bottomLeftCorner(m - 1, 1) = e; + // generate first Householder vector + VectorType e; + RealScalar beta; + r0.makeHouseholder(e, tau.coeffRef(0), beta); + w(0)=(Scalar) beta; + H.bottomLeftCorner(m - 1, 1) = e; - for (int k = 1; k <= restart; ++k) { + for (int k = 1; k <= restart; ++k) { - ++iters; + ++iters; - VectorType v = VectorType::Unit(m, k - 1), workspace(m); + VectorType v = VectorType::Unit(m, k - 1), workspace(m); - // apply Householder reflections H_{1} ... H_{k-1} to v - for (int i = k - 1; i >= 0; --i) { - v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); - } + // apply Householder reflections H_{1} ... H_{k-1} to v + for (int i = k - 1; i >= 0; --i) { + v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } - // apply matrix M to v: v = mat * v; - VectorType t=mat*v; - v=precond.solve(t); + // apply matrix M to v: v = mat * v; + VectorType t=mat*v; + v=precond.solve(t); - // apply Householder reflections H_{k-1} ... H_{1} to v - for (int i = 0; i < k; ++i) { - v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); - } + // apply Householder reflections H_{k-1} ... H_{1} to v + for (int i = 0; i < k; ++i) { + v.tail(m - i).applyHouseholderOnTheLeft(H.col(i).tail(m - i - 1), tau.coeffRef(i), workspace.data()); + } - if (v.tail(m - k).norm() != 0.0) { + if (v.tail(m - k).norm() != 0.0) { - if (k <= restart) { + if (k <= restart) { - // generate new Householder vector + // generate new Householder vector VectorType e(m - k - 1); - RealScalar beta; - v.tail(m - k).makeHouseholder(e, tau.coeffRef(k), beta); - H.col(k).tail(m - k - 1) = e; + RealScalar beta; + v.tail(m - k).makeHouseholder(e, tau.coeffRef(k), beta); + H.col(k).tail(m - k - 1) = e; - // apply Householder reflection H_{k} to v - v.tail(m - k).applyHouseholderOnTheLeft(H.col(k).tail(m - k - 1), tau.coeffRef(k), workspace.data()); + // apply Householder reflection H_{k} to v + v.tail(m - k).applyHouseholderOnTheLeft(H.col(k).tail(m - k - 1), tau.coeffRef(k), workspace.data()); - } + } } if (k > 1) { @@ -207,9 +207,9 @@ bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Precondition - } - - return false; + } + + return false; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index 6cdd65748..e328badda 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -248,7 +248,7 @@ template EIGEN_STRONG_INLINE void MatrixExponential::pade9(const MatrixType &A) { const RealScalar b[] = {17643225600., 8821612800., 2075673600., 302702400., 30270240., - 2162160., 110880., 3960., 90., 1.}; + 2162160., 110880., 3960., 90., 1.}; MatrixType A2 = A * A; MatrixType A4 = A2 * A2; MatrixType A6 = A4 * A2; @@ -262,8 +262,8 @@ template EIGEN_STRONG_INLINE void MatrixExponential::pade13(const MatrixType &A) { const RealScalar b[] = {64764752532480000., 32382376266240000., 7771770303897600., - 1187353796428800., 129060195264000., 10559470521600., 670442572800., - 33522128640., 1323241920., 40840800., 960960., 16380., 182., 1.}; + 1187353796428800., 129060195264000., 10559470521600., 670442572800., + 33522128640., 1323241920., 40840800., 960960., 16380., 182., 1.}; MatrixType A2 = A * A; MatrixType A4 = A2 * A2; m_tmp1.noalias() = A4 * A2; diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h index 859de7288..7c493e24d 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h @@ -47,7 +47,7 @@ namespace Eigen { * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic */ template ::Scalar>::IsComplex> class MatrixFunction { @@ -267,13 +267,13 @@ void MatrixFunction::partitionEigenvalues() // Look for other element to add to the set for (Index j=i+1; jbegin(), qi->end(), diag(j)) == qi->end()) { - typename ListOfClusters::iterator qj = findCluster(diag(j)); - if (qj == m_clusters.end()) { - qi->push_back(diag(j)); - } else { - qi->insert(qi->end(), qj->begin(), qj->end()); - m_clusters.erase(qj); - } + typename ListOfClusters::iterator qj = findCluster(diag(j)); + if (qj == m_clusters.end()) { + qi->push_back(diag(j)); + } else { + qi->insert(qi->end(), qj->begin(), qj->end()); + m_clusters.erase(qj); + } } } } @@ -412,8 +412,8 @@ void MatrixFunction::computeOffDiagonal() DynMatrixType C = block(m_fT, blockIndex, blockIndex) * block(m_T, blockIndex, blockIndex+diagIndex); C -= block(m_T, blockIndex, blockIndex+diagIndex) * block(m_fT, blockIndex+diagIndex, blockIndex+diagIndex); for (Index k = blockIndex + 1; k < blockIndex + diagIndex; k++) { - C += block(m_fT, blockIndex, k) * block(m_T, k, blockIndex+diagIndex); - C -= block(m_T, blockIndex, k) * block(m_fT, k, blockIndex+diagIndex); + C += block(m_fT, blockIndex, k) * block(m_T, k, blockIndex+diagIndex); + C -= block(m_T, blockIndex, k) * block(m_fT, k, blockIndex+diagIndex); } block(m_fT, blockIndex, blockIndex+diagIndex) = solveTriangularSylvester(A, B, C); } @@ -466,19 +466,19 @@ typename MatrixFunction::DynMatrixType MatrixFunction AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i); - AX = AXmatrix(0,0); + Matrix AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i); + AX = AXmatrix(0,0); } // Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj} Scalar XB; if (j == 0) { - XB = 0; + XB = 0; } else { - Matrix XBmatrix = X.row(i).head(j) * B.col(j).head(j); - XB = XBmatrix(0,0); + Matrix XBmatrix = X.row(i).head(j) * B.col(j).head(j); + XB = XBmatrix(0,0); } X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j)); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h index 97ab662fe..078545b90 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h @@ -118,7 +118,7 @@ void MatrixFunctionAtomic::computeMu() /** \brief Determine whether Taylor series has converged */ template bool MatrixFunctionAtomic::taylorConverged(Index s, const MatrixType& F, - const MatrixType& Fincr, const MatrixType& P) + const MatrixType& Fincr, const MatrixType& P) { const Index n = F.rows(); const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff(); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h index e6f25b73c..361c3df0c 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h @@ -168,7 +168,7 @@ void MatrixLogarithmAtomic::computeBig(const MatrixType& A, MatrixTy degree = getPadeDegree(normTminusI); int degree2 = getPadeDegree(normTminusI / RealScalar(2)); if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) - break; + break; ++numberOfExtraSquareRoots; } MatrixType sqrtT; @@ -309,10 +309,10 @@ void MatrixLogarithmAtomic::computePade6(MatrixType& result, const M const int degree = 6; const RealScalar nodes[] = { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L, 0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L, - 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L }; + 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L }; const RealScalar weights[] = { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L, 0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L, - 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L }; + 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L }; assert(degree <= maxPadeDegree); MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows()); result.setZero(T.rows(), T.rows()); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h index 658cd334c..2f8f28363 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h @@ -75,17 +75,17 @@ class MatrixSquareRootQuasiTriangular void computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, const MatrixType& T); void compute2x2diagonalBlock(MatrixType& sqrtT, const MatrixType& T, typename MatrixType::Index i); void compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); void compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); void compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); void compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j); + typename MatrixType::Index i, typename MatrixType::Index j); template static void solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A, - const SmallMatrixType& B, const SmallMatrixType& C); + const SmallMatrixType& B, const SmallMatrixType& C); const MatrixType& m_A; }; @@ -112,7 +112,7 @@ void MatrixSquareRootQuasiTriangular::compute(ResultType &result) // post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T template void MatrixSquareRootQuasiTriangular::computeDiagonalPartOfSqrt(MatrixType& sqrtT, - const MatrixType& T) + const MatrixType& T) { const Index size = m_A.rows(); for (Index i = 0; i < size; i++) { @@ -131,25 +131,25 @@ void MatrixSquareRootQuasiTriangular::computeDiagonalPartOfSqrt(Matr // post: sqrtT is the square root of T. template void MatrixSquareRootQuasiTriangular::computeOffDiagonalPartOfSqrt(MatrixType& sqrtT, - const MatrixType& T) + const MatrixType& T) { const Index size = m_A.rows(); for (Index j = 1; j < size; j++) { if (T.coeff(j, j-1) != 0) // if T(j-1:j, j-1:j) is a 2-by-2 block - continue; + continue; for (Index i = j-1; i >= 0; i--) { if (i > 0 && T.coeff(i, i-1) != 0) // if T(i-1:i, i-1:i) is a 2-by-2 block - continue; + continue; bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0); bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0); if (iBlockIs2x2 && jBlockIs2x2) - compute2x2offDiagonalBlock(sqrtT, T, i, j); + compute2x2offDiagonalBlock(sqrtT, T, i, j); else if (iBlockIs2x2 && !jBlockIs2x2) - compute2x1offDiagonalBlock(sqrtT, T, i, j); + compute2x1offDiagonalBlock(sqrtT, T, i, j); else if (!iBlockIs2x2 && jBlockIs2x2) - compute1x2offDiagonalBlock(sqrtT, T, i, j); + compute1x2offDiagonalBlock(sqrtT, T, i, j); else if (!iBlockIs2x2 && !jBlockIs2x2) - compute1x1offDiagonalBlock(sqrtT, T, i, j); + compute1x1offDiagonalBlock(sqrtT, T, i, j); } } } @@ -174,7 +174,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute1x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value(); sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j)); @@ -184,7 +184,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Matrix rhs = T.template block<1,2>(i,j); if (j-i > 1) @@ -198,7 +198,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Matrix rhs = T.template block<2,1>(i,j); if (j-i > 2) @@ -212,7 +212,7 @@ void MatrixSquareRootQuasiTriangular template void MatrixSquareRootQuasiTriangular ::compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T, - typename MatrixType::Index i, typename MatrixType::Index j) + typename MatrixType::Index i, typename MatrixType::Index j) { Matrix A = sqrtT.template block<2,2>(i,i); Matrix B = sqrtT.template block<2,2>(j,j); @@ -229,10 +229,10 @@ template template void MatrixSquareRootQuasiTriangular ::solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A, - const SmallMatrixType& B, const SmallMatrixType& C) + const SmallMatrixType& B, const SmallMatrixType& C) { EIGEN_STATIC_ASSERT((internal::is_same >::value), - EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT); + EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT); Matrix coeffMatrix = Matrix::Zero(); coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h index 3de68ec3a..84f0df583 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h @@ -47,17 +47,17 @@ class StdStemFunctions Scalar res; switch (n % 4) { case 0: - res = std::cos(x); - break; + res = std::cos(x); + break; case 1: - res = -std::sin(x); - break; + res = -std::sin(x); + break; case 2: - res = -std::cos(x); - break; + res = -std::cos(x); + break; case 3: - res = std::sin(x); - break; + res = std::sin(x); + break; } return res; } @@ -68,17 +68,17 @@ class StdStemFunctions Scalar res; switch (n % 4) { case 0: - res = std::sin(x); - break; + res = std::sin(x); + break; case 1: - res = std::cos(x); - break; + res = std::cos(x); + break; case 2: - res = -std::sin(x); - break; + res = -std::sin(x); + break; case 3: - res = -std::cos(x); - break; + res = -std::cos(x); + break; } return res; } @@ -89,26 +89,26 @@ class StdStemFunctions Scalar res; switch (n % 2) { case 0: - res = std::cosh(x); - break; + res = std::cosh(x); + break; case 1: - res = std::sinh(x); - break; + res = std::sinh(x); + break; } return res; } - + /** \brief Hyperbolic sine (and its derivatives). */ static Scalar sinh(Scalar x, int n) { Scalar res; switch (n % 2) { case 0: - res = std::sinh(x); - break; + res = std::sinh(x); + break; case 1: - res = std::cosh(x); - break; + res = std::cosh(x); + break; } return res; } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h index 9cfe1d9f5..adaf33a8e 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h @@ -253,9 +253,9 @@ bool saveMarket(const SparseMatrixType& mat, const std::string& filename, int sy for(int j=0; j typename EigenSolver::EigenvalueType eivals = es.eigenvalues(); for (typename MatrixType::Index i = 0; i < size; ++i) { if (eivals(i).imag() == 0 && eivals(i).real() < 0) - eivals(i) = -eivals(i); + eivals(i) = -eivals(i); } result = (es.eigenvectors() * eivals.asDiagonal() * es.eigenvectors().inverse()).real(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp index 5c23544ef..44012de3b 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.cpp @@ -1,60 +1,60 @@ /* - Multi-precision real number class. C++ interface fo MPFR library. - Project homepage: http://www.holoborodko.com/pavel/ - Contact e-mail: pavel@holoborodko.com + Multi-precision real number class. C++ interface fo MPFR library. + Project homepage: http://www.holoborodko.com/pavel/ + Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2011 Pavel Holoborodko + Copyright (c) 2008-2011 Pavel Holoborodko - Core Developers: - Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. + Core Developers: + Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. - Contributors: - Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, - Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, - Tsai Chia Cheng, Alexei Zubanov. + Contributors: + Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, + Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, + Tsai Chia Cheng, Alexei Zubanov. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + **************************************************************************** + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - **************************************************************************** - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: + **************************************************************************** + **************************************************************************** + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. - 3. The name of the author may be used to endorse or promote products - derived from this software without specific prior written permission. + 3. The name of the author may be used to endorse or promote products + derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. */ #include #include "mpreal.h" @@ -72,9 +72,9 @@ using std::istream; namespace mpfr{ -mp_rnd_t mpreal::default_rnd = MPFR_RNDN; //(mpfr_get_default_rounding_mode)(); -mp_prec_t mpreal::default_prec = 64; //(mpfr_get_default_prec)(); -int mpreal::default_base = 10; +mp_rnd_t mpreal::default_rnd = MPFR_RNDN; //(mpfr_get_default_rounding_mode)(); +mp_prec_t mpreal::default_prec = 64; //(mpfr_get_default_prec)(); +int mpreal::default_base = 10; int mpreal::double_bits = -1; #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) @@ -86,93 +86,93 @@ mpreal::mpreal() { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,default_prec); - mpfr_set_ui(mp,0,default_rnd); + mpfr_init2(mp,default_prec); + mpfr_set_ui(mp,0,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpreal& u) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,mpfr_get_prec(u.mp)); - mpfr_set(mp,u.mp,default_rnd); + mpfr_init2(mp,mpfr_get_prec(u.mp)); + mpfr_set(mp,u.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpfr_t u) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,mpfr_get_prec(u)); - mpfr_set(mp,u,default_rnd); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,mpfr_get_prec(u)); + mpfr_set(mp,u,default_rnd); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpf_t u) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) - mpfr_set_f(mp,u,default_rnd); + mpfr_init2(mp,(mp_prec_t) mpf_get_prec(u)); // (gmp: mp_bitcnt_t) unsigned long -> long (mpfr: mp_prec_t) + mpfr_set_f(mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpz_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_z(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_z(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const mpq_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_q(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_q(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const double u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif if(double_bits == -1 || fits_in_bits(u, double_bits)) { - mpfr_init2(mp,prec); - mpfr_set_d(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_d(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } else throw conversion_overflow(); @@ -182,65 +182,65 @@ mpreal::mpreal(const long double u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif mpfr_init2(mp,prec); - mpfr_set_ld(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_set_ld(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const unsigned long int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_ui(mp,u,mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const unsigned int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_ui(mp,u,mode); + mpfr_init2(mp,prec); + mpfr_set_ui(mp,u,mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const long int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2(mp,prec); + mpfr_set_si(mp,u,mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const int u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_si(mp,u,mode); + mpfr_init2(mp,prec); + mpfr_set_si(mp,u,mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } #if defined (MPREAL_HAVE_INT64_SUPPORT) @@ -248,26 +248,26 @@ mpreal::mpreal(const uint64_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_uj(mp, u, mode); + mpfr_init2(mp,prec); + mpfr_set_uj(mp, u, mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const int64_t u, mp_prec_t prec, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_sj(mp, u, mode); - - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_init2(mp,prec); + mpfr_set_sj(mp, u, mode); + + MPREAL_MSVC_DEBUGVIEW_CODE; } #endif @@ -275,157 +275,157 @@ mpreal::mpreal(const char* s, mp_prec_t prec, int base, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_str(mp, s, base, mode); + mpfr_init2(mp,prec); + mpfr_set_str(mp, s, base, mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::mpreal(const std::string& s, mp_prec_t prec, int base, mp_rnd_t mode) { #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - mpfr_init2(mp,prec); - mpfr_set_str(mp, s.c_str(), base, mode); + mpfr_init2(mp,prec); + mpfr_set_str(mp, s.c_str(), base, mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } mpreal::~mpreal() { - mpfr_clear(mp); + mpfr_clear(mp); } // Operators - Assignment mpreal& mpreal::operator=(const char* s) { - mpfr_t t; + mpfr_t t; #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - if(0==mpfr_init_set_str(t,s,default_base,default_rnd)) - { - // We will rewrite mp anyway, so flash it and resize - mpfr_set_prec(mp,mpfr_get_prec(t)); - mpfr_set(mp,t,mpreal::default_rnd); - mpfr_clear(t); + if(0==mpfr_init_set_str(t,s,default_base,default_rnd)) + { + // We will rewrite mp anyway, so flash it and resize + mpfr_set_prec(mp,mpfr_get_prec(t)); + mpfr_set(mp,t,mpreal::default_rnd); + mpfr_clear(t); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; - }else{ - mpfr_clear(t); - } + }else{ + mpfr_clear(t); + } - return *this; + return *this; } const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t p1, p2, p3; + mpreal a; + mp_prec_t p1, p2, p3; - p1 = v1.get_prec(); - p2 = v2.get_prec(); - p3 = v3.get_prec(); + p1 = v1.get_prec(); + p2 = v2.get_prec(); + p3 = v3.get_prec(); - a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); + a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); - mpfr_fma(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); - return a; + mpfr_fma(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); + return a; } const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t p1, p2, p3; + mpreal a; + mp_prec_t p1, p2, p3; - p1 = v1.get_prec(); - p2 = v2.get_prec(); - p3 = v3.get_prec(); + p1 = v1.get_prec(); + p2 = v2.get_prec(); + p3 = v3.get_prec(); - a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); + a.set_prec(p3>p2?(p3>p1?p3:p1):(p2>p1?p2:p1)); - mpfr_fms(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); - return a; + mpfr_fms(a.mp,v1.mp,v2.mp,v3.mp,rnd_mode); + return a; } const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t p1, p2; + mpreal a; + mp_prec_t p1, p2; - p1 = v1.get_prec(); - p2 = v2.get_prec(); + p1 = v1.get_prec(); + p2 = v2.get_prec(); - a.set_prec(p1>p2?p1:p2); + a.set_prec(p1>p2?p1:p2); - mpfr_agm(a.mp, v1.mp, v2.mp, rnd_mode); + mpfr_agm(a.mp, v1.mp, v2.mp, rnd_mode); - return a; + return a; } const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode) { - mpreal x; - mpfr_ptr* t; - unsigned long int i; + mpreal x; + mpfr_ptr* t; + unsigned long int i; - t = new mpfr_ptr[n]; - for (i=0;ixp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); + mpfr_remquo(a.mp,q, x.mp, y.mp, rnd_mode); - return a; + return a; } template std::string toString(T t, std::ios_base & (*f)(std::ios_base&)) { - std::ostringstream oss; - oss << f << t; - return oss.str(); + std::ostringstream oss; + oss << f << t; + return oss.str(); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) std::string mpreal::toString(const std::string& format) const { - char *s = NULL; - string out; + char *s = NULL; + string out; - if( !format.empty() ) - { - if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) - { - out = std::string(s); + if( !format.empty() ) + { + if(!(mpfr_asprintf(&s,format.c_str(),mp) < 0)) + { + out = std::string(s); - mpfr_free_str(s); - } - } + mpfr_free_str(s); + } + } - return out; + return out; } #endif @@ -436,116 +436,116 @@ std::string mpreal::toString(int n, int b, mp_rnd_t mode) const (void)mode; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - // Use MPFR native function for output - char format[128]; - int digits; + // Use MPFR native function for output + char format[128]; + int digits; - digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); + digits = n > 0 ? n : bits2digits(mpfr_get_prec(mp)); - sprintf(format,"%%.%dRNg",digits); // Default format + sprintf(format,"%%.%dRNg",digits); // Default format - return toString(std::string(format)); + return toString(std::string(format)); #else - char *s, *ns = NULL; - size_t slen, nslen; - mp_exp_t exp; - string out; + char *s, *ns = NULL; + size_t slen, nslen; + mp_exp_t exp; + string out; #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - set_custom_malloc(); + set_custom_malloc(); #endif - if(mpfr_inf_p(mp)) - { - if(mpfr_sgn(mp)>0) return "+Inf"; - else return "-Inf"; - } + if(mpfr_inf_p(mp)) + { + if(mpfr_sgn(mp)>0) return "+Inf"; + else return "-Inf"; + } - if(mpfr_zero_p(mp)) return "0"; - if(mpfr_nan_p(mp)) return "NaN"; + if(mpfr_zero_p(mp)) return "0"; + if(mpfr_nan_p(mp)) return "NaN"; - s = mpfr_get_str(NULL,&exp,b,0,mp,mode); - ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); + s = mpfr_get_str(NULL,&exp,b,0,mp,mode); + ns = mpfr_get_str(NULL,&exp,b,n,mp,mode); - if(s!=NULL && ns!=NULL) - { - slen = strlen(s); - nslen = strlen(ns); - if(nslen<=slen) - { - mpfr_free_str(s); - s = ns; - slen = nslen; - } - else { - mpfr_free_str(ns); - } + if(s!=NULL && ns!=NULL) + { + slen = strlen(s); + nslen = strlen(ns); + if(nslen<=slen) + { + mpfr_free_str(s); + s = ns; + slen = nslen; + } + else { + mpfr_free_str(ns); + } - // Make human eye-friendly formatting if possible - if (exp>0 && static_cast(exp)s+exp) ptr--; + // Make human eye-friendly formatting if possible + if (exp>0 && static_cast(exp)s+exp) ptr--; - if(ptr==s+exp) out = string(s,exp+1); - else out = string(s,exp+1)+'.'+string(s+exp+1,ptr-(s+exp+1)+1); + if(ptr==s+exp) out = string(s,exp+1); + else out = string(s,exp+1)+'.'+string(s+exp+1,ptr-(s+exp+1)+1); - //out = string(s,exp+1)+'.'+string(s+exp+1); - } - else - { - // Remove zeros starting from right end - char* ptr = s+slen-1; - while (*ptr=='0' && ptr>s+exp-1) ptr--; + //out = string(s,exp+1)+'.'+string(s+exp+1); + } + else + { + // Remove zeros starting from right end + char* ptr = s+slen-1; + while (*ptr=='0' && ptr>s+exp-1) ptr--; - if(ptr==s+exp-1) out = string(s,exp); - else out = string(s,exp)+'.'+string(s+exp,ptr-(s+exp)+1); + if(ptr==s+exp-1) out = string(s,exp); + else out = string(s,exp)+'.'+string(s+exp,ptr-(s+exp)+1); - //out = string(s,exp)+'.'+string(s+exp); - } + //out = string(s,exp)+'.'+string(s+exp); + } - }else{ // exp<0 || exp>slen - if(s[0]=='-') - { - // Remove zeros starting from right end - char* ptr = s+slen-1; - while (*ptr=='0' && ptr>s+1) ptr--; + }else{ // exp<0 || exp>slen + if(s[0]=='-') + { + // Remove zeros starting from right end + char* ptr = s+slen-1; + while (*ptr=='0' && ptr>s+1) ptr--; - if(ptr==s+1) out = string(s,2); - else out = string(s,2)+'.'+string(s+2,ptr-(s+2)+1); + if(ptr==s+1) out = string(s,2); + else out = string(s,2)+'.'+string(s+2,ptr-(s+2)+1); - //out = string(s,2)+'.'+string(s+2); - } - else - { - // Remove zeros starting from right end - char* ptr = s+slen-1; - while (*ptr=='0' && ptr>s) ptr--; + //out = string(s,2)+'.'+string(s+2); + } + else + { + // Remove zeros starting from right end + char* ptr = s+slen-1; + while (*ptr=='0' && ptr>s) ptr--; - if(ptr==s) out = string(s,1); - else out = string(s,1)+'.'+string(s+1,ptr-(s+1)+1); + if(ptr==s) out = string(s,1); + else out = string(s,1)+'.'+string(s+1,ptr-(s+1)+1); - //out = string(s,1)+'.'+string(s+1); - } + //out = string(s,1)+'.'+string(s+1); + } - // Make final string - if(--exp) - { - if(exp>0) out += "e+"+mpfr::toString(exp,std::dec); - else out += "e"+mpfr::toString(exp,std::dec); - } - } + // Make final string + if(--exp) + { + if(exp>0) out += "e+"+mpfr::toString(exp,std::dec); + else out += "e"+mpfr::toString(exp,std::dec); + } + } - mpfr_free_str(s); - return out; - }else{ - return "conversion error!"; - } + mpfr_free_str(s); + return out; + }else{ + return "conversion error!"; + } #endif } @@ -554,43 +554,43 @@ std::string mpreal::toString(int n, int b, mp_rnd_t mode) const // I/O ostream& operator<<(ostream& os, const mpreal& v) { - return os<(os.precision())); + return os<(os.precision())); } istream& operator>>(istream &is, mpreal& v) { - string tmp; - is >> tmp; - mpfr_set_str(v.mp, tmp.c_str(),mpreal::default_base,mpreal::default_rnd); - return is; + string tmp; + is >> tmp; + mpfr_set_str(v.mp, tmp.c_str(),mpreal::default_base,mpreal::default_rnd); + return is; } #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) - // Optimized dynamic memory allocation/(re-)deallocation. - void * mpreal::mpreal_allocate(size_t alloc_size) - { - return(dlmalloc(alloc_size)); - } + // Optimized dynamic memory allocation/(re-)deallocation. + void * mpreal::mpreal_allocate(size_t alloc_size) + { + return(dlmalloc(alloc_size)); + } - void * mpreal::mpreal_reallocate(void *ptr, size_t old_size, size_t new_size) - { - return(dlrealloc(ptr,new_size)); - } + void * mpreal::mpreal_reallocate(void *ptr, size_t old_size, size_t new_size) + { + return(dlrealloc(ptr,new_size)); + } - void mpreal::mpreal_free(void *ptr, size_t size) - { - dlfree(ptr); - } + void mpreal::mpreal_free(void *ptr, size_t size) + { + dlfree(ptr); + } - inline void mpreal::set_custom_malloc(void) - { - if(!is_custom_malloc) - { - mp_set_memory_functions(mpreal_allocate,mpreal_reallocate,mpreal_free); - is_custom_malloc = true; - } - } + inline void mpreal::set_custom_malloc(void) + { + if(!is_custom_malloc) + { + mp_set_memory_functions(mpreal_allocate,mpreal_reallocate,mpreal_free); + is_custom_malloc = true; + } + } #endif } diff --git a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h index c640af947..9245beab0 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h +++ b/gtsam/3rdparty/Eigen/unsupported/test/mpreal/mpreal.h @@ -1,59 +1,59 @@ /* - Multi-precision real number class. C++ interface for MPFR library. - Project homepage: http://www.holoborodko.com/pavel/ - Contact e-mail: pavel@holoborodko.com + Multi-precision real number class. C++ interface for MPFR library. + Project homepage: http://www.holoborodko.com/pavel/ + Contact e-mail: pavel@holoborodko.com - Copyright (c) 2008-2012 Pavel Holoborodko + Copyright (c) 2008-2012 Pavel Holoborodko - Core Developers: - Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. + Core Developers: + Pavel Holoborodko, Dmitriy Gubanov, Konstantin Holoborodko. - Contributors: - Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, - Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, - Tsai Chia Cheng, Alexei Zubanov. + Contributors: + Brian Gladman, Helmut Jarausch, Fokko Beekhof, Ulrich Mutze, + Heinz van Saanen, Pere Constans, Peter van Hoof, Gael Guennebaud, + Tsai Chia Cheng, Alexei Zubanov. - **************************************************************************** - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + **************************************************************************** + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - **************************************************************************** - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - 3. The name of the author may be used to endorse or promote products - derived from this software without specific prior written permission. + **************************************************************************** + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + 3. The name of the author may be used to endorse or promote products + derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + SUCH DAMAGE. */ #ifndef __MPREAL_H__ @@ -67,544 +67,544 @@ #include // Options -#define MPREAL_HAVE_INT64_SUPPORT // int64_t support: available only for MSVC 2010 & GCC -#define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer (valid only for MSVC in "Debug" builds) +#define MPREAL_HAVE_INT64_SUPPORT // int64_t support: available only for MSVC 2010 & GCC +#define MPREAL_HAVE_MSVC_DEBUGVIEW // Enable Debugger Visualizer (valid only for MSVC in "Debug" builds) // Detect compiler using signatures from http://predef.sourceforge.net/ #if defined(__GNUC__) && defined(__INTEL_COMPILER) - #define IsInf(x) isinf(x) // Intel ICC compiler on Linux + #define IsInf(x) isinf(x) // Intel ICC compiler on Linux -#elif defined(_MSC_VER) // Microsoft Visual C++ - #define IsInf(x) (!_finite(x)) +#elif defined(_MSC_VER) // Microsoft Visual C++ + #define IsInf(x) (!_finite(x)) #elif defined(__GNUC__) - #define IsInf(x) std::isinf(x) // GNU C/C++ + #define IsInf(x) std::isinf(x) // GNU C/C++ #else - #define IsInf(x) std::isinf(x) // Unknown compiler, just hope for C99 conformance + #define IsInf(x) std::isinf(x) // Unknown compiler, just hope for C99 conformance #endif #if defined(MPREAL_HAVE_INT64_SUPPORT) - - #define MPFR_USE_INTMAX_T // should be defined before mpfr.h + + #define MPFR_USE_INTMAX_T // should be defined before mpfr.h - #if defined(_MSC_VER) // is available only in msvc2010! - #if (_MSC_VER >= 1600) - #include - #else // MPFR relies on intmax_t which is available only in msvc2010 - #undef MPREAL_HAVE_INT64_SUPPORT // Besides, MPFR - MPIR have to be compiled with msvc2010 - #undef MPFR_USE_INTMAX_T // Since we cannot detect this, disable x64 by default - // Someone should change this manually if needed. - #endif - #endif - - #if defined (__MINGW32__) || defined(__MINGW64__) - #include // equivalent to msvc2010 - #elif defined (__GNUC__) - #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) - #undef MPREAL_HAVE_INT64_SUPPORT // remove all shaman dances for x64 builds since - #undef MPFR_USE_INTMAX_T // GCC already support x64 as of "long int" is 64-bit integer, nothing left to do - #else - #include // use int64_t, uint64_t otherwise. - #endif - #endif + #if defined(_MSC_VER) // is available only in msvc2010! + #if (_MSC_VER >= 1600) + #include + #else // MPFR relies on intmax_t which is available only in msvc2010 + #undef MPREAL_HAVE_INT64_SUPPORT // Besides, MPFR - MPIR have to be compiled with msvc2010 + #undef MPFR_USE_INTMAX_T // Since we cannot detect this, disable x64 by default + // Someone should change this manually if needed. + #endif + #endif + + #if defined (__MINGW32__) || defined(__MINGW64__) + #include // equivalent to msvc2010 + #elif defined (__GNUC__) + #if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) + #undef MPREAL_HAVE_INT64_SUPPORT // remove all shaman dances for x64 builds since + #undef MPFR_USE_INTMAX_T // GCC already support x64 as of "long int" is 64-bit integer, nothing left to do + #else + #include // use int64_t, uint64_t otherwise. + #endif + #endif #endif #if defined(MPREAL_HAVE_MSVC_DEBUGVIEW) && defined(_MSC_VER) && defined(_DEBUG) -#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString() - #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView +#define MPREAL_MSVC_DEBUGVIEW_CODE DebugView = toString() + #define MPREAL_MSVC_DEBUGVIEW_DATA std::string DebugView #else - #define MPREAL_MSVC_DEBUGVIEW_CODE - #define MPREAL_MSVC_DEBUGVIEW_DATA + #define MPREAL_MSVC_DEBUGVIEW_CODE + #define MPREAL_MSVC_DEBUGVIEW_DATA #endif #include #if (MPFR_VERSION < MPFR_VERSION_NUM(3,0,0)) - #include // needed for random() + #include // needed for random() #endif namespace mpfr { class mpreal { private: - mpfr_t mp; + mpfr_t mp; public: - static mp_rnd_t default_rnd; - static mp_prec_t default_prec; - static int default_base; - static int double_bits; + static mp_rnd_t default_rnd; + static mp_prec_t default_prec; + static int default_base; + static int double_bits; public: - // Constructors && type conversion - mpreal(); - mpreal(const mpreal& u); - mpreal(const mpfr_t u); - mpreal(const mpf_t u); - mpreal(const mpz_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const mpq_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const long double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const unsigned long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const unsigned int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + // Constructors && type conversion + mpreal(); + mpreal(const mpreal& u); + mpreal(const mpfr_t u); + mpreal(const mpf_t u); + mpreal(const mpz_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const mpq_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const long double u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const unsigned long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const unsigned int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const long int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const int u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); #if defined (MPREAL_HAVE_INT64_SUPPORT) - mpreal(const uint64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); - mpreal(const int64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const uint64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); + mpreal(const int64_t u, mp_prec_t prec = default_prec, mp_rnd_t mode = default_rnd); #endif - mpreal(const char* s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); - mpreal(const std::string& s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); + mpreal(const char* s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); + mpreal(const std::string& s, mp_prec_t prec = default_prec, int base = default_base, mp_rnd_t mode = default_rnd); - ~mpreal(); + ~mpreal(); - // Operations - // = - // +, -, *, /, ++, --, <<, >> - // *=, +=, -=, /=, - // <, >, ==, <=, >= + // Operations + // = + // +, -, *, /, ++, --, <<, >> + // *=, +=, -=, /=, + // <, >, ==, <=, >= - // = - mpreal& operator=(const mpreal& v); - mpreal& operator=(const mpf_t v); - mpreal& operator=(const mpz_t v); - mpreal& operator=(const mpq_t v); - mpreal& operator=(const long double v); - mpreal& operator=(const double v); - mpreal& operator=(const unsigned long int v); - mpreal& operator=(const unsigned int v); - mpreal& operator=(const long int v); - mpreal& operator=(const int v); - mpreal& operator=(const char* s); + // = + mpreal& operator=(const mpreal& v); + mpreal& operator=(const mpf_t v); + mpreal& operator=(const mpz_t v); + mpreal& operator=(const mpq_t v); + mpreal& operator=(const long double v); + mpreal& operator=(const double v); + mpreal& operator=(const unsigned long int v); + mpreal& operator=(const unsigned int v); + mpreal& operator=(const long int v); + mpreal& operator=(const int v); + mpreal& operator=(const char* s); - // + - mpreal& operator+=(const mpreal& v); - mpreal& operator+=(const mpf_t v); - mpreal& operator+=(const mpz_t v); - mpreal& operator+=(const mpq_t v); - mpreal& operator+=(const long double u); - mpreal& operator+=(const double u); - mpreal& operator+=(const unsigned long int u); - mpreal& operator+=(const unsigned int u); - mpreal& operator+=(const long int u); - mpreal& operator+=(const int u); + // + + mpreal& operator+=(const mpreal& v); + mpreal& operator+=(const mpf_t v); + mpreal& operator+=(const mpz_t v); + mpreal& operator+=(const mpq_t v); + mpreal& operator+=(const long double u); + mpreal& operator+=(const double u); + mpreal& operator+=(const unsigned long int u); + mpreal& operator+=(const unsigned int u); + mpreal& operator+=(const long int u); + mpreal& operator+=(const int u); #if defined (MPREAL_HAVE_INT64_SUPPORT) - mpreal& operator+=(const int64_t u); - mpreal& operator+=(const uint64_t u); - mpreal& operator-=(const int64_t u); - mpreal& operator-=(const uint64_t u); - mpreal& operator*=(const int64_t u); - mpreal& operator*=(const uint64_t u); - mpreal& operator/=(const int64_t u); - mpreal& operator/=(const uint64_t u); + mpreal& operator+=(const int64_t u); + mpreal& operator+=(const uint64_t u); + mpreal& operator-=(const int64_t u); + mpreal& operator-=(const uint64_t u); + mpreal& operator*=(const int64_t u); + mpreal& operator*=(const uint64_t u); + mpreal& operator/=(const int64_t u); + mpreal& operator/=(const uint64_t u); #endif - const mpreal operator+() const; - mpreal& operator++ (); - const mpreal operator++ (int); + const mpreal operator+() const; + mpreal& operator++ (); + const mpreal operator++ (int); - // - - mpreal& operator-=(const mpreal& v); - mpreal& operator-=(const mpz_t v); - mpreal& operator-=(const mpq_t v); - mpreal& operator-=(const long double u); - mpreal& operator-=(const double u); - mpreal& operator-=(const unsigned long int u); - mpreal& operator-=(const unsigned int u); - mpreal& operator-=(const long int u); - mpreal& operator-=(const int u); - const mpreal operator-() const; - friend const mpreal operator-(const unsigned long int b, const mpreal& a); - friend const mpreal operator-(const unsigned int b, const mpreal& a); - friend const mpreal operator-(const long int b, const mpreal& a); - friend const mpreal operator-(const int b, const mpreal& a); - friend const mpreal operator-(const double b, const mpreal& a); - mpreal& operator-- (); - const mpreal operator-- (int); + // - + mpreal& operator-=(const mpreal& v); + mpreal& operator-=(const mpz_t v); + mpreal& operator-=(const mpq_t v); + mpreal& operator-=(const long double u); + mpreal& operator-=(const double u); + mpreal& operator-=(const unsigned long int u); + mpreal& operator-=(const unsigned int u); + mpreal& operator-=(const long int u); + mpreal& operator-=(const int u); + const mpreal operator-() const; + friend const mpreal operator-(const unsigned long int b, const mpreal& a); + friend const mpreal operator-(const unsigned int b, const mpreal& a); + friend const mpreal operator-(const long int b, const mpreal& a); + friend const mpreal operator-(const int b, const mpreal& a); + friend const mpreal operator-(const double b, const mpreal& a); + mpreal& operator-- (); + const mpreal operator-- (int); - // * - mpreal& operator*=(const mpreal& v); - mpreal& operator*=(const mpz_t v); - mpreal& operator*=(const mpq_t v); - mpreal& operator*=(const long double v); - mpreal& operator*=(const double v); - mpreal& operator*=(const unsigned long int v); - mpreal& operator*=(const unsigned int v); - mpreal& operator*=(const long int v); - mpreal& operator*=(const int v); - - // / - mpreal& operator/=(const mpreal& v); - mpreal& operator/=(const mpz_t v); - mpreal& operator/=(const mpq_t v); - mpreal& operator/=(const long double v); - mpreal& operator/=(const double v); - mpreal& operator/=(const unsigned long int v); - mpreal& operator/=(const unsigned int v); - mpreal& operator/=(const long int v); - mpreal& operator/=(const int v); - friend const mpreal operator/(const unsigned long int b, const mpreal& a); - friend const mpreal operator/(const unsigned int b, const mpreal& a); - friend const mpreal operator/(const long int b, const mpreal& a); - friend const mpreal operator/(const int b, const mpreal& a); - friend const mpreal operator/(const double b, const mpreal& a); + // * + mpreal& operator*=(const mpreal& v); + mpreal& operator*=(const mpz_t v); + mpreal& operator*=(const mpq_t v); + mpreal& operator*=(const long double v); + mpreal& operator*=(const double v); + mpreal& operator*=(const unsigned long int v); + mpreal& operator*=(const unsigned int v); + mpreal& operator*=(const long int v); + mpreal& operator*=(const int v); + + // / + mpreal& operator/=(const mpreal& v); + mpreal& operator/=(const mpz_t v); + mpreal& operator/=(const mpq_t v); + mpreal& operator/=(const long double v); + mpreal& operator/=(const double v); + mpreal& operator/=(const unsigned long int v); + mpreal& operator/=(const unsigned int v); + mpreal& operator/=(const long int v); + mpreal& operator/=(const int v); + friend const mpreal operator/(const unsigned long int b, const mpreal& a); + friend const mpreal operator/(const unsigned int b, const mpreal& a); + friend const mpreal operator/(const long int b, const mpreal& a); + friend const mpreal operator/(const int b, const mpreal& a); + friend const mpreal operator/(const double b, const mpreal& a); - //<<= Fast Multiplication by 2^u - mpreal& operator<<=(const unsigned long int u); - mpreal& operator<<=(const unsigned int u); - mpreal& operator<<=(const long int u); - mpreal& operator<<=(const int u); + //<<= Fast Multiplication by 2^u + mpreal& operator<<=(const unsigned long int u); + mpreal& operator<<=(const unsigned int u); + mpreal& operator<<=(const long int u); + mpreal& operator<<=(const int u); - //>>= Fast Division by 2^u - mpreal& operator>>=(const unsigned long int u); - mpreal& operator>>=(const unsigned int u); - mpreal& operator>>=(const long int u); - mpreal& operator>>=(const int u); + //>>= Fast Division by 2^u + mpreal& operator>>=(const unsigned long int u); + mpreal& operator>>=(const unsigned int u); + mpreal& operator>>=(const long int u); + mpreal& operator>>=(const int u); - // Boolean Operators - friend bool operator > (const mpreal& a, const mpreal& b); - friend bool operator >= (const mpreal& a, const mpreal& b); - friend bool operator < (const mpreal& a, const mpreal& b); - friend bool operator <= (const mpreal& a, const mpreal& b); - friend bool operator == (const mpreal& a, const mpreal& b); - friend bool operator != (const mpreal& a, const mpreal& b); + // Boolean Operators + friend bool operator > (const mpreal& a, const mpreal& b); + friend bool operator >= (const mpreal& a, const mpreal& b); + friend bool operator < (const mpreal& a, const mpreal& b); + friend bool operator <= (const mpreal& a, const mpreal& b); + friend bool operator == (const mpreal& a, const mpreal& b); + friend bool operator != (const mpreal& a, const mpreal& b); - // Optimized specializations for boolean operators - friend bool operator == (const mpreal& a, const unsigned long int b); - friend bool operator == (const mpreal& a, const unsigned int b); - friend bool operator == (const mpreal& a, const long int b); - friend bool operator == (const mpreal& a, const int b); - friend bool operator == (const mpreal& a, const long double b); - friend bool operator == (const mpreal& a, const double b); + // Optimized specializations for boolean operators + friend bool operator == (const mpreal& a, const unsigned long int b); + friend bool operator == (const mpreal& a, const unsigned int b); + friend bool operator == (const mpreal& a, const long int b); + friend bool operator == (const mpreal& a, const int b); + friend bool operator == (const mpreal& a, const long double b); + friend bool operator == (const mpreal& a, const double b); - // Type Conversion operators - long toLong() const; - unsigned long toULong() const; - double toDouble() const; - long double toLDouble() const; + // Type Conversion operators + long toLong() const; + unsigned long toULong() const; + double toDouble() const; + long double toLDouble() const; #if defined (MPREAL_HAVE_INT64_SUPPORT) - int64_t toInt64() const; - uint64_t toUInt64() const; + int64_t toInt64() const; + uint64_t toUInt64() const; #endif - // Get raw pointers - ::mpfr_ptr mpfr_ptr(); - ::mpfr_srcptr mpfr_srcptr() const; + // Get raw pointers + ::mpfr_ptr mpfr_ptr(); + ::mpfr_srcptr mpfr_srcptr() const; - // Convert mpreal to string with n significant digits in base b - // n = 0 -> convert with the maximum available digits - std::string toString(int n = 0, int b = default_base, mp_rnd_t mode = default_rnd) const; + // Convert mpreal to string with n significant digits in base b + // n = 0 -> convert with the maximum available digits + std::string toString(int n = 0, int b = default_base, mp_rnd_t mode = default_rnd) const; #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - std::string toString(const std::string& format) const; + std::string toString(const std::string& format) const; #endif - // Math Functions - friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + // Math Functions + friend const mpreal sqr (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const mpreal& a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal pow (const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend int cmpabs(const mpreal& a,const mpreal& b); - - friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int cmpabs(const mpreal& a,const mpreal& b); + + friend const mpreal log (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal log2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal exp (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal exp2 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal log1p(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal expm1(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fac_ui (unsigned long int v, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend int sgn(const mpreal& v); // -1 or +1 + friend const mpreal gamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal lgamma (const mpreal& v, int *signp = 0, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fma (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fms (const mpreal& v1, const mpreal& v2, const mpreal& v3, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal agm (const mpreal& v1, const mpreal& v2, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal sum (const mpreal tab[], unsigned long int n, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int sgn(const mpreal& v); // -1 or +1 // MPFR 2.4.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - friend int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal li2(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal li2(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); #endif // MPFR 3.0.0 Specifics #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - friend const mpreal digamma(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal urandom (gmp_randstate_t& state,mp_rnd_t rnd_mode = mpreal::default_rnd); // use gmp_randinit_default() to init state, gmp_randclear() to clear - friend bool isregular(const mpreal& v); + friend const mpreal digamma(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal urandom (gmp_randstate_t& state,mp_rnd_t rnd_mode = mpreal::default_rnd); // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend bool isregular(const mpreal& v); #endif - - // Uniformly distributed random number generation in [0,1] using - // Mersenne-Twister algorithm by default. - // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) - // Check urandom() for more precise control. - friend const mpreal random(unsigned int seed = 0); - - // Exponent and mantissa manipulation - friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); - friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); + + // Uniformly distributed random number generation in [0,1] using + // Mersenne-Twister algorithm by default. + // Use parameter to setup seed, e.g.: random((unsigned)time(NULL)) + // Check urandom() for more precise control. + friend const mpreal random(unsigned int seed = 0); + + // Exponent and mantissa manipulation + friend const mpreal frexp(const mpreal& v, mp_exp_t* exp); + friend const mpreal ldexp(const mpreal& v, mp_exp_t exp); - // Splits mpreal value into fractional and integer parts. - // Returns fractional part and stores integer part in n. - friend const mpreal modf(const mpreal& v, mpreal& n); + // Splits mpreal value into fractional and integer parts. + // Returns fractional part and stores integer part in n. + friend const mpreal modf(const mpreal& v, mpreal& n); - // Constants - // don't forget to call mpfr_free_cache() for every thread where you are using const-functions - friend const mpreal const_log2 (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal const_pi (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal const_euler (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal const_catalan (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - // returns +inf iff sign>=0 otherwise -inf - friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + // Constants + // don't forget to call mpfr_free_cache() for every thread where you are using const-functions + friend const mpreal const_log2 (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal const_pi (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal const_euler (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal const_catalan (mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); + // returns +inf iff sign>=0 otherwise -inf + friend const mpreal const_infinity(int sign = 1, mp_prec_t prec = mpreal::default_prec, mp_rnd_t rnd_mode = mpreal::default_rnd); - // Output/ Input - friend std::ostream& operator<<(std::ostream& os, const mpreal& v); + // Output/ Input + friend std::ostream& operator<<(std::ostream& os, const mpreal& v); friend std::istream& operator>>(std::istream& is, mpreal& v); - // Integer Related Functions - friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal ceil (const mpreal& v); - friend const mpreal floor(const mpreal& v); - friend const mpreal round(const mpreal& v); - friend const mpreal trunc(const mpreal& v); - friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rint_floor(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rint_round(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal rint_trunc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); - - // Miscellaneous Functions - friend const mpreal nexttoward (const mpreal& x, const mpreal& y); - friend const mpreal nextabove (const mpreal& x); - friend const mpreal nextbelow (const mpreal& x); + // Integer Related Functions + friend const mpreal rint (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal ceil (const mpreal& v); + friend const mpreal floor(const mpreal& v); + friend const mpreal round(const mpreal& v); + friend const mpreal trunc(const mpreal& v); + friend const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rint_floor(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rint_round(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal rint_trunc(const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + friend const mpreal remquo (long* q, const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = mpreal::default_rnd); + + // Miscellaneous Functions + friend const mpreal nexttoward (const mpreal& x, const mpreal& y); + friend const mpreal nextabove (const mpreal& x); + friend const mpreal nextbelow (const mpreal& x); - // use gmp_randinit_default() to init state, gmp_randclear() to clear - friend const mpreal urandomb (gmp_randstate_t& state); + // use gmp_randinit_default() to init state, gmp_randclear() to clear + friend const mpreal urandomb (gmp_randstate_t& state); // MPFR < 2.4.2 Specifics #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) - friend const mpreal random2 (mp_size_t size, mp_exp_t exp); + friend const mpreal random2 (mp_size_t size, mp_exp_t exp); #endif - // Instance Checkers - friend bool isnan (const mpreal& v); - friend bool isinf (const mpreal& v); - friend bool isfinite(const mpreal& v); + // Instance Checkers + friend bool isnan (const mpreal& v); + friend bool isinf (const mpreal& v); + friend bool isfinite(const mpreal& v); - friend bool isnum(const mpreal& v); - friend bool iszero(const mpreal& v); - friend bool isint(const mpreal& v); + friend bool isnum(const mpreal& v); + friend bool iszero(const mpreal& v); + friend bool isint(const mpreal& v); - // Set/Get instance properties - inline mp_prec_t get_prec() const; - inline void set_prec(mp_prec_t prec, mp_rnd_t rnd_mode = default_rnd); // Change precision with rounding mode + // Set/Get instance properties + inline mp_prec_t get_prec() const; + inline void set_prec(mp_prec_t prec, mp_rnd_t rnd_mode = default_rnd); // Change precision with rounding mode - // Aliases for get_prec(), set_prec() - needed for compatibility with std::complex interface - inline mpreal& setPrecision(int Precision, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); - inline int getPrecision() const; - - // Set mpreal to +/- inf, NaN, +/-0 - mpreal& setInf (int Sign = +1); - mpreal& setNan (); - mpreal& setZero (int Sign = +1); - mpreal& setSign (int Sign, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); + // Aliases for get_prec(), set_prec() - needed for compatibility with std::complex interface + inline mpreal& setPrecision(int Precision, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); + inline int getPrecision() const; + + // Set mpreal to +/- inf, NaN, +/-0 + mpreal& setInf (int Sign = +1); + mpreal& setNan (); + mpreal& setZero (int Sign = +1); + mpreal& setSign (int Sign, mp_rnd_t RoundingMode = (mpfr_get_default_rounding_mode)()); - //Exponent - mp_exp_t get_exp(); - int set_exp(mp_exp_t e); - int check_range (int t, mp_rnd_t rnd_mode = default_rnd); - int subnormalize (int t,mp_rnd_t rnd_mode = default_rnd); + //Exponent + mp_exp_t get_exp(); + int set_exp(mp_exp_t e); + int check_range (int t, mp_rnd_t rnd_mode = default_rnd); + int subnormalize (int t,mp_rnd_t rnd_mode = default_rnd); - // Inexact conversion from float - inline bool fits_in_bits(double x, int n); + // Inexact conversion from float + inline bool fits_in_bits(double x, int n); - // Set/Get global properties - static void set_default_prec(mp_prec_t prec); - static mp_prec_t get_default_prec(); - static void set_default_base(int base); - static int get_default_base(); - static void set_double_bits(int dbits); - static int get_double_bits(); - static void set_default_rnd(mp_rnd_t rnd_mode); - static mp_rnd_t get_default_rnd(); - static mp_exp_t get_emin (void); - static mp_exp_t get_emax (void); - static mp_exp_t get_emin_min (void); - static mp_exp_t get_emin_max (void); - static mp_exp_t get_emax_min (void); - static mp_exp_t get_emax_max (void); - static int set_emin (mp_exp_t exp); - static int set_emax (mp_exp_t exp); + // Set/Get global properties + static void set_default_prec(mp_prec_t prec); + static mp_prec_t get_default_prec(); + static void set_default_base(int base); + static int get_default_base(); + static void set_double_bits(int dbits); + static int get_double_bits(); + static void set_default_rnd(mp_rnd_t rnd_mode); + static mp_rnd_t get_default_rnd(); + static mp_exp_t get_emin (void); + static mp_exp_t get_emax (void); + static mp_exp_t get_emin_min (void); + static mp_exp_t get_emin_max (void); + static mp_exp_t get_emax_min (void); + static mp_exp_t get_emax_max (void); + static int set_emin (mp_exp_t exp); + static int set_emax (mp_exp_t exp); - // Efficient swapping of two mpreal values - friend void swap(mpreal& x, mpreal& y); - - //Min Max - macros is evil. Needed for systems which defines max and min globally as macros (e.g. Windows) - //Hope that globally defined macros use > < operations only - friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); - friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); + // Efficient swapping of two mpreal values + friend void swap(mpreal& x, mpreal& y); + + //Min Max - macros is evil. Needed for systems which defines max and min globally as macros (e.g. Windows) + //Hope that globally defined macros use > < operations only + friend const mpreal fmax(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); + friend const mpreal fmin(const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode = default_rnd); #if defined (MPREAL_HAVE_CUSTOM_MPFR_MALLOC) private: - // Optimized dynamic memory allocation/(re-)deallocation. - static bool is_custom_malloc; - static void *mpreal_allocate (size_t alloc_size); - static void *mpreal_reallocate (void *ptr, size_t old_size, size_t new_size); - static void mpreal_free (void *ptr, size_t size); - inline static void set_custom_malloc (void); + // Optimized dynamic memory allocation/(re-)deallocation. + static bool is_custom_malloc; + static void *mpreal_allocate (size_t alloc_size); + static void *mpreal_reallocate (void *ptr, size_t old_size, size_t new_size); + static void mpreal_free (void *ptr, size_t size); + inline static void set_custom_malloc (void); #endif private: - // Human friendly Debug Preview in Visual Studio. - // Put one of these lines: - // - // mpfr::mpreal= ; Show value only - // mpfr::mpreal=, bits ; Show value & precision - // - // at the beginning of - // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat - MPREAL_MSVC_DEBUGVIEW_DATA + // Human friendly Debug Preview in Visual Studio. + // Put one of these lines: + // + // mpfr::mpreal= ; Show value only + // mpfr::mpreal=, bits ; Show value & precision + // + // at the beginning of + // [Visual Studio Installation Folder]\Common7\Packages\Debugger\autoexp.dat + MPREAL_MSVC_DEBUGVIEW_DATA }; ////////////////////////////////////////////////////////////////////////// // Exceptions class conversion_overflow : public std::exception { public: - std::string why() { return "inexact conversion from floating point"; } + std::string why() { return "inexact conversion from floating point"; } }; namespace internal{ - // Use SFINAE to restrict arithmetic operations instantiation only for numeric types - // This is needed for smooth integration with libraries based on expression templates - template struct result_type {}; - - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; + // Use SFINAE to restrict arithmetic operations instantiation only for numeric types + // This is needed for smooth integration with libraries based on expression templates + template struct result_type {}; + + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; #if defined (MPREAL_HAVE_INT64_SUPPORT) - template <> struct result_type {typedef mpreal type;}; - template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; + template <> struct result_type {typedef mpreal type;}; #endif } // + Addition template inline const typename internal::result_type::type - operator+(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) += rhs; } + operator+(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) += rhs; } template inline const typename internal::result_type::type - operator+(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) += lhs; } + operator+(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) += lhs; } // - Subtraction template inline const typename internal::result_type::type - operator-(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) -= rhs; } + operator-(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) -= rhs; } template inline const typename internal::result_type::type - operator-(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) -= rhs; } + operator-(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) -= rhs; } // * Multiplication template inline const typename internal::result_type::type - operator*(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) *= rhs; } + operator*(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) *= rhs; } template inline const typename internal::result_type::type - operator*(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) *= lhs; } + operator*(const Lhs& lhs, const mpreal& rhs){ return mpreal(rhs) *= lhs; } // / Division template inline const typename internal::result_type::type - operator/(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) /= rhs; } + operator/(const mpreal& lhs, const Rhs& rhs){ return mpreal(lhs) /= rhs; } template inline const typename internal::result_type::type - operator/(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) /= rhs; } + operator/(const Lhs& lhs, const mpreal& rhs){ return mpreal(lhs) /= rhs; } ////////////////////////////////////////////////////////////////////////// // sqrt @@ -654,13 +654,13 @@ const mpreal pow(const int a, const int b, mp_rnd_t rnd_mode = mpreal::default_r const mpreal pow(const int a, const long double b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const int a, const double b, mp_rnd_t rnd_mode = mpreal::default_rnd); -const mpreal pow(const long double a, const long double b, mp_rnd_t rnd_mode = mpreal::default_rnd); +const mpreal pow(const long double a, const long double b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const long double a, const int b, mp_rnd_t rnd_mode = mpreal::default_rnd); -const mpreal pow(const double a, const double b, mp_rnd_t rnd_mode = mpreal::default_rnd); +const mpreal pow(const double a, const double b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const double a, const unsigned long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode = mpreal::default_rnd); const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode = mpreal::default_rnd); @@ -672,7 +672,7 @@ const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode = mpreal::defaul inline const mpreal machine_epsilon(mp_prec_t prec = mpreal::get_default_prec()); // Returns the positive distance from abs(x) to the next larger in magnitude floating point number of the same precision as x -inline const mpreal machine_epsilon(const mpreal& x); +inline const mpreal machine_epsilon(const mpreal& x); inline const mpreal mpreal_min(mp_prec_t prec = mpreal::get_default_prec()); inline const mpreal mpreal_max(mp_prec_t prec = mpreal::get_default_prec()); @@ -680,9 +680,9 @@ inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps); inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps); ////////////////////////////////////////////////////////////////////////// -// Bits - decimal digits relation -// bits = ceil(digits*log[2](10)) -// digits = floor(bits*log[10](2)) +// Bits - decimal digits relation +// bits = ceil(digits*log[2](10)) +// digits = floor(bits*log[10](2)) inline mp_prec_t digits2bits(int d); inline int bits2digits(mp_prec_t b); @@ -700,541 +700,541 @@ const mpreal (min)(const mpreal& x, const mpreal& y); // Operators - Assignment inline mpreal& mpreal::operator=(const mpreal& v) { - if (this != &v) - { - mpfr_clear(mp); - mpfr_init2(mp,mpfr_get_prec(v.mp)); - mpfr_set(mp,v.mp,default_rnd); + if (this != &v) + { + mpfr_clear(mp); + mpfr_init2(mp,mpfr_get_prec(v.mp)); + mpfr_set(mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - } - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + } + return *this; } inline mpreal& mpreal::operator=(const mpf_t v) { - mpfr_set_f(mp,v,default_rnd); - - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_f(mp,v,default_rnd); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator=(const mpz_t v) { - mpfr_set_z(mp,v,default_rnd); - - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_z(mp,v,default_rnd); + + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator=(const mpq_t v) { - mpfr_set_q(mp,v,default_rnd); + mpfr_set_q(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const long double v) -{ +inline mpreal& mpreal::operator=(const long double v) +{ mpfr_set_ld(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const double v) -{ +inline mpreal& mpreal::operator=(const double v) +{ if(double_bits == -1 || fits_in_bits(v, double_bits)) { - mpfr_set_d(mp,v,default_rnd); + mpfr_set_d(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; + MPREAL_MSVC_DEBUGVIEW_CODE; } else throw conversion_overflow(); - return *this; + return *this; } -inline mpreal& mpreal::operator=(const unsigned long int v) -{ - mpfr_set_ui(mp,v,default_rnd); +inline mpreal& mpreal::operator=(const unsigned long int v) +{ + mpfr_set_ui(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const unsigned int v) -{ - mpfr_set_ui(mp,v,default_rnd); +inline mpreal& mpreal::operator=(const unsigned int v) +{ + mpfr_set_ui(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::operator=(const long int v) -{ - mpfr_set_si(mp,v,default_rnd); +inline mpreal& mpreal::operator=(const long int v) +{ + mpfr_set_si(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator=(const int v) -{ - mpfr_set_si(mp,v,default_rnd); +{ + mpfr_set_si(mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } ////////////////////////////////////////////////////////////////////////// // + Addition inline mpreal& mpreal::operator+=(const mpreal& v) { - mpfr_add(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const mpf_t u) { - *this += mpreal(u); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this += mpreal(u); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const mpz_t u) { - mpfr_add_z(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_z(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const mpq_t u) { - mpfr_add_q(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_q(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+= (const long double u) { - *this += mpreal(u); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this += mpreal(u); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+= (const double u) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_add_d(mp,mp,u,default_rnd); + mpfr_add_d(mp,mp,u,default_rnd); #else - *this += mpreal(u); + *this += mpreal(u); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const unsigned long int u) { - mpfr_add_ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const unsigned int u) { - mpfr_add_ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const long int u) { - mpfr_add_si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator+=(const int u) { - mpfr_add_si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_add_si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline mpreal& mpreal::operator+=(const int64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator+=(const uint64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator-=(const int64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator-=(const uint64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator*=(const int64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator*=(const uint64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator/=(const int64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } -inline mpreal& mpreal::operator/=(const uint64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator+=(const int64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator+=(const uint64_t u){ *this += mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator-=(const int64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator-=(const uint64_t u){ *this -= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator*=(const int64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator*=(const uint64_t u){ *this *= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator/=(const int64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } +inline mpreal& mpreal::operator/=(const uint64_t u){ *this /= mpreal(u); MPREAL_MSVC_DEBUGVIEW_CODE; return *this; } #endif -inline const mpreal mpreal::operator+()const { return mpreal(*this); } +inline const mpreal mpreal::operator+()const { return mpreal(*this); } inline const mpreal operator+(const mpreal& a, const mpreal& b) { - // prec(a+b) = max(prec(a),prec(b)) - if(a.get_prec()>b.get_prec()) return mpreal(a) += b; - else return mpreal(b) += a; + // prec(a+b) = max(prec(a),prec(b)) + if(a.get_prec()>b.get_prec()) return mpreal(a) += b; + else return mpreal(b) += a; } inline mpreal& mpreal::operator++() { - return *this += 1; + return *this += 1; } inline const mpreal mpreal::operator++ (int) { - mpreal x(*this); - *this += 1; - return x; + mpreal x(*this); + *this += 1; + return x; } inline mpreal& mpreal::operator--() { - return *this -= 1; + return *this -= 1; } inline const mpreal mpreal::operator-- (int) { - mpreal x(*this); - *this -= 1; - return x; + mpreal x(*this); + *this -= 1; + return x; } ////////////////////////////////////////////////////////////////////////// // - Subtraction inline mpreal& mpreal::operator-= (const mpreal& v) { - mpfr_sub(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const mpz_t v) { - mpfr_sub_z(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_z(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const mpq_t v) { - mpfr_sub_q(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_q(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const long double v) { - *this -= mpreal(v); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this -= mpreal(v); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_sub_d(mp,mp,v,default_rnd); + mpfr_sub_d(mp,mp,v,default_rnd); #else - *this -= mpreal(v); + *this -= mpreal(v); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const unsigned long int v) { - mpfr_sub_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const unsigned int v) { - mpfr_sub_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const long int v) { - mpfr_sub_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator-=(const int v) { - mpfr_sub_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_sub_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal mpreal::operator-()const { - mpreal u(*this); - mpfr_neg(u.mp,u.mp,default_rnd); - return u; + mpreal u(*this); + mpfr_neg(u.mp,u.mp,default_rnd); + return u; } inline const mpreal operator-(const mpreal& a, const mpreal& b) { - // prec(a-b) = max(prec(a),prec(b)) - if(a.getPrecision() >= b.getPrecision()) - { - return mpreal(a) -= b; - }else{ - mpreal x(a); - x.setPrecision(b.getPrecision()); - return x -= b; - } + // prec(a-b) = max(prec(a),prec(b)) + if(a.getPrecision() >= b.getPrecision()) + { + return mpreal(a) -= b; + }else{ + mpreal x(a); + x.setPrecision(b.getPrecision()); + return x -= b; + } } inline const mpreal operator-(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(a); - mpfr_d_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_d_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; #else - return mpreal(b) -= a; + return mpreal(b) -= a; #endif } inline const mpreal operator-(const unsigned long int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator-(const unsigned int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator-(const long int b, const mpreal& a) { - mpreal x(a); - mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator-(const int b, const mpreal& a) { - mpreal x(a); - mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_sub(x.mp,b,a.mp,mpreal::default_rnd); + return x; } ////////////////////////////////////////////////////////////////////////// // * Multiplication inline mpreal& mpreal::operator*= (const mpreal& v) { - mpfr_mul(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const mpz_t v) { - mpfr_mul_z(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_z(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const mpq_t v) { - mpfr_mul_q(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_q(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const long double v) { - *this *= mpreal(v); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this *= mpreal(v); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_mul_d(mp,mp,v,default_rnd); + mpfr_mul_d(mp,mp,v,default_rnd); #else - *this *= mpreal(v); + *this *= mpreal(v); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const unsigned long int v) { - mpfr_mul_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const unsigned int v) { - mpfr_mul_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const long int v) { - mpfr_mul_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator*=(const int v) { - mpfr_mul_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal operator*(const mpreal& a, const mpreal& b) { - // prec(a*b) = max(prec(a),prec(b)) - if(a.getPrecision() >= b.getPrecision()) return mpreal(a) *= b; - else return mpreal(b) *= a; + // prec(a*b) = max(prec(a),prec(b)) + if(a.getPrecision() >= b.getPrecision()) return mpreal(a) *= b; + else return mpreal(b) *= a; } ////////////////////////////////////////////////////////////////////////// // / Division inline mpreal& mpreal::operator/=(const mpreal& v) { - mpfr_div(mp,mp,v.mp,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div(mp,mp,v.mp,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const mpz_t v) { - mpfr_div_z(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_z(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const mpq_t v) { - mpfr_div_q(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_q(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const long double v) { - *this /= mpreal(v); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + *this /= mpreal(v); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const double v) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpfr_div_d(mp,mp,v,default_rnd); + mpfr_div_d(mp,mp,v,default_rnd); #else - *this /= mpreal(v); + *this /= mpreal(v); #endif - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const unsigned long int v) { - mpfr_div_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const unsigned int v) { - mpfr_div_ui(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_ui(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const long int v) { - mpfr_div_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator/=(const int v) { - mpfr_div_si(mp,mp,v,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_si(mp,mp,v,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal operator/(const mpreal& a, const mpreal& b) { - // prec(a/b) = max(prec(a),prec(b)) - if(a.getPrecision() >= b.getPrecision()) - { - return mpreal(a) /= b; - }else{ + // prec(a/b) = max(prec(a),prec(b)) + if(a.getPrecision() >= b.getPrecision()) + { + return mpreal(a) /= b; + }else{ - mpreal x(a); - x.setPrecision(b.getPrecision()); - return x /= b; - } + mpreal x(a); + x.setPrecision(b.getPrecision()); + return x /= b; + } } inline const mpreal operator/(const unsigned long int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const unsigned int b, const mpreal& a) { - mpreal x(a); - mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_ui_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const long int b, const mpreal& a) { - mpreal x(a); - mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const int b, const mpreal& a) { - mpreal x(a); - mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_si_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; } inline const mpreal operator/(const double b, const mpreal& a) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(2,4,0)) - mpreal x(a); - mpfr_d_div(x.mp,b,a.mp,mpreal::default_rnd); - return x; + mpreal x(a); + mpfr_d_div(x.mp,b,a.mp,mpreal::default_rnd); + return x; #else - return mpreal(b) /= a; + return mpreal(b) /= a; #endif } @@ -1242,314 +1242,314 @@ inline const mpreal operator/(const double b, const mpreal& a) // Shifts operators - Multiplication/Division by power of 2 inline mpreal& mpreal::operator<<=(const unsigned long int u) { - mpfr_mul_2ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator<<=(const unsigned int u) { - mpfr_mul_2ui(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2ui(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator<<=(const long int u) { - mpfr_mul_2si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator<<=(const int u) { - mpfr_mul_2si(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_mul_2si(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const unsigned long int u) { - mpfr_div_2ui(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2ui(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const unsigned int u) { - mpfr_div_2ui(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2ui(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const long int u) { - mpfr_div_2si(mp,mp,u,default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2si(mp,mp,u,default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::operator>>=(const int u) { - mpfr_div_2si(mp,mp,static_cast(u),default_rnd); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_div_2si(mp,mp,static_cast(u),default_rnd); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline const mpreal operator<<(const mpreal& v, const unsigned long int k) { - return mul_2ui(v,k); + return mul_2ui(v,k); } inline const mpreal operator<<(const mpreal& v, const unsigned int k) { - return mul_2ui(v,static_cast(k)); + return mul_2ui(v,static_cast(k)); } inline const mpreal operator<<(const mpreal& v, const long int k) { - return mul_2si(v,k); + return mul_2si(v,k); } inline const mpreal operator<<(const mpreal& v, const int k) { - return mul_2si(v,static_cast(k)); + return mul_2si(v,static_cast(k)); } inline const mpreal operator>>(const mpreal& v, const unsigned long int k) { - return div_2ui(v,k); + return div_2ui(v,k); } inline const mpreal operator>>(const mpreal& v, const long int k) { - return div_2si(v,k); + return div_2si(v,k); } inline const mpreal operator>>(const mpreal& v, const unsigned int k) { - return div_2ui(v,static_cast(k)); + return div_2ui(v,static_cast(k)); } inline const mpreal operator>>(const mpreal& v, const int k) { - return div_2si(v,static_cast(k)); + return div_2si(v,static_cast(k)); } // mul_2ui inline const mpreal mul_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_mul_2ui(x.mp,v.mp,k,rnd_mode); + return x; } // mul_2si inline const mpreal mul_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_mul_2si(x.mp,v.mp,k,rnd_mode); + return x; } inline const mpreal div_2ui(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_div_2ui(x.mp,v.mp,k,rnd_mode); + return x; } inline const mpreal div_2si(const mpreal& v, long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_div_2si(x.mp,v.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_div_2si(x.mp,v.mp,k,rnd_mode); + return x; } ////////////////////////////////////////////////////////////////////////// //Boolean operators -inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } -inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } -inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } -inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } -inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } +inline bool operator > (const mpreal& a, const mpreal& b){ return (mpfr_greater_p(a.mp,b.mp) !=0); } +inline bool operator >= (const mpreal& a, const mpreal& b){ return (mpfr_greaterequal_p(a.mp,b.mp) !=0); } +inline bool operator < (const mpreal& a, const mpreal& b){ return (mpfr_less_p(a.mp,b.mp) !=0); } +inline bool operator <= (const mpreal& a, const mpreal& b){ return (mpfr_lessequal_p(a.mp,b.mp) !=0); } +inline bool operator == (const mpreal& a, const mpreal& b){ return (mpfr_equal_p(a.mp,b.mp) !=0); } +inline bool operator != (const mpreal& a, const mpreal& b){ return (mpfr_lessgreater_p(a.mp,b.mp) !=0); } -inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } -inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned long int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const unsigned int b ){ return (mpfr_cmp_ui(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const long int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const int b ){ return (mpfr_cmp_si(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const long double b ){ return (mpfr_cmp_ld(a.mp,b) == 0); } +inline bool operator == (const mpreal& a, const double b ){ return (mpfr_cmp_d(a.mp,b) == 0); } -inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } -inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } -inline bool isfinite(const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } -inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } -inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } +inline bool isnan (const mpreal& v){ return (mpfr_nan_p(v.mp) != 0); } +inline bool isinf (const mpreal& v){ return (mpfr_inf_p(v.mp) != 0); } +inline bool isfinite(const mpreal& v){ return (mpfr_number_p(v.mp) != 0); } +inline bool iszero (const mpreal& v){ return (mpfr_zero_p(v.mp) != 0); } +inline bool isint (const mpreal& v){ return (mpfr_integer_p(v.mp) != 0); } #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) -inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} +inline bool isregular(const mpreal& v){ return (mpfr_regular_p(v.mp));} #endif ////////////////////////////////////////////////////////////////////////// // Type Converters -inline long mpreal::toLong() const { return mpfr_get_si(mp,GMP_RNDZ); } -inline unsigned long mpreal::toULong() const { return mpfr_get_ui(mp,GMP_RNDZ); } -inline double mpreal::toDouble() const { return mpfr_get_d(mp,default_rnd); } -inline long double mpreal::toLDouble() const { return mpfr_get_ld(mp,default_rnd); } +inline long mpreal::toLong() const { return mpfr_get_si(mp,GMP_RNDZ); } +inline unsigned long mpreal::toULong() const { return mpfr_get_ui(mp,GMP_RNDZ); } +inline double mpreal::toDouble() const { return mpfr_get_d(mp,default_rnd); } +inline long double mpreal::toLDouble() const { return mpfr_get_ld(mp,default_rnd); } #if defined (MPREAL_HAVE_INT64_SUPPORT) -inline int64_t mpreal::toInt64() const{ return mpfr_get_sj(mp,GMP_RNDZ); } -inline uint64_t mpreal::toUInt64() const{ return mpfr_get_uj(mp,GMP_RNDZ); } +inline int64_t mpreal::toInt64() const{ return mpfr_get_sj(mp,GMP_RNDZ); } +inline uint64_t mpreal::toUInt64() const{ return mpfr_get_uj(mp,GMP_RNDZ); } #endif -inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } -inline ::mpfr_srcptr mpreal::mpfr_srcptr() const { return const_cast< ::mpfr_srcptr >(mp); } +inline ::mpfr_ptr mpreal::mpfr_ptr() { return mp; } +inline ::mpfr_srcptr mpreal::mpfr_srcptr() const { return const_cast< ::mpfr_srcptr >(mp); } ////////////////////////////////////////////////////////////////////////// -// Bits - decimal digits relation -// bits = ceil(digits*log[2](10)) -// digits = floor(bits*log[10](2)) +// Bits - decimal digits relation +// bits = ceil(digits*log[2](10)) +// digits = floor(bits*log[10](2)) inline mp_prec_t digits2bits(int d) { - const double LOG2_10 = 3.3219280948873624; + const double LOG2_10 = 3.3219280948873624; - d = 10>d?10:d; + d = 10>d?10:d; - return (mp_prec_t)std::ceil((d)*LOG2_10); + return (mp_prec_t)std::ceil((d)*LOG2_10); } inline int bits2digits(mp_prec_t b) { - const double LOG10_2 = 0.30102999566398119; + const double LOG10_2 = 0.30102999566398119; - b = 34>b?34:b; + b = 34>b?34:b; - return (int)std::floor((b)*LOG10_2); + return (int)std::floor((b)*LOG10_2); } ////////////////////////////////////////////////////////////////////////// // Set/Get number properties inline int sgn(const mpreal& v) { - int r = mpfr_signbit(v.mp); - return (r>0?-1:1); + int r = mpfr_signbit(v.mp); + return (r>0?-1:1); } inline mpreal& mpreal::setSign(int sign, mp_rnd_t RoundingMode) { - mpfr_setsign(mp,mp,(sign<0?1:0),RoundingMode); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_setsign(mp,mp,(sign<0?1:0),RoundingMode); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline int mpreal::getPrecision() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mp); } inline mpreal& mpreal::setPrecision(int Precision, mp_rnd_t RoundingMode) { - mpfr_prec_round(mp,Precision, RoundingMode); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_prec_round(mp,Precision, RoundingMode); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mpreal& mpreal::setInf(int sign) { - mpfr_set_inf(mp,sign); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; -} + mpfr_set_inf(mp,sign); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; +} inline mpreal& mpreal::setNan() { - mpfr_set_nan(mp); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_nan(mp); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } -inline mpreal& mpreal::setZero(int sign) +inline mpreal& mpreal::setZero(int sign) { - mpfr_set_zero(mp,sign); - MPREAL_MSVC_DEBUGVIEW_CODE; - return *this; + mpfr_set_zero(mp,sign); + MPREAL_MSVC_DEBUGVIEW_CODE; + return *this; } inline mp_prec_t mpreal::get_prec() const { - return mpfr_get_prec(mp); + return mpfr_get_prec(mp); } inline void mpreal::set_prec(mp_prec_t prec, mp_rnd_t rnd_mode) { - mpfr_prec_round(mp,prec,rnd_mode); - MPREAL_MSVC_DEBUGVIEW_CODE; + mpfr_prec_round(mp,prec,rnd_mode); + MPREAL_MSVC_DEBUGVIEW_CODE; } inline mp_exp_t mpreal::get_exp () { - return mpfr_get_exp(mp); + return mpfr_get_exp(mp); } inline int mpreal::set_exp (mp_exp_t e) { - int x = mpfr_set_exp(mp, e); - MPREAL_MSVC_DEBUGVIEW_CODE; - return x; + int x = mpfr_set_exp(mp, e); + MPREAL_MSVC_DEBUGVIEW_CODE; + return x; } inline const mpreal frexp(const mpreal& v, mp_exp_t* exp) { - mpreal x(v); - *exp = x.get_exp(); - x.set_exp(0); - return x; + mpreal x(v); + *exp = x.get_exp(); + x.set_exp(0); + return x; } inline const mpreal ldexp(const mpreal& v, mp_exp_t exp) { - mpreal x(v); + mpreal x(v); - // rounding is not important since we just increasing the exponent - mpfr_mul_2si(x.mp,x.mp,exp,mpreal::default_rnd); - return x; + // rounding is not important since we just increasing the exponent + mpfr_mul_2si(x.mp,x.mp,exp,mpreal::default_rnd); + return x; } inline const mpreal machine_epsilon(mp_prec_t prec) { - // the smallest eps such that 1.0+eps != 1.0 - // depends (of cause) on the precision - return machine_epsilon(mpreal(1,prec)); + // the smallest eps such that 1.0+eps != 1.0 + // depends (of cause) on the precision + return machine_epsilon(mpreal(1,prec)); } inline const mpreal machine_epsilon(const mpreal& x) -{ - if( x < 0) - { - return nextabove(-x)+x; - }else{ - return nextabove(x)-x; - } +{ + if( x < 0) + { + return nextabove(-x)+x; + }else{ + return nextabove(x)-x; + } } inline const mpreal mpreal_min(mp_prec_t prec) { - // min = 1/2*2^emin = 2^(emin-1) + // min = 1/2*2^emin = 2^(emin-1) - return mpreal(1,prec) << mpreal::get_emin()-1; + return mpreal(1,prec) << mpreal::get_emin()-1; } inline const mpreal mpreal_max(mp_prec_t prec) { - // max = (1-eps)*2^emax, assume eps = 0?, - // and use emax-1 to prevent value to be +inf - // max = 2^(emax-1) + // max = (1-eps)*2^emax, assume eps = 0?, + // and use emax-1 to prevent value to be +inf + // max = 2^(emax-1) - return mpreal(1,prec) << mpreal::get_emax()-1; + return mpreal(1,prec) << mpreal::get_emax()-1; } inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) @@ -1562,74 +1562,74 @@ inline bool isEqualUlps(const mpreal& a, const mpreal& b, int maxUlps) inline bool isEqualFuzzy(const mpreal& a, const mpreal& b, const mpreal& eps) { - return abs(a - b) <= (min)(abs(a), abs(b)) * eps; + return abs(a - b) <= (min)(abs(a), abs(b)) * eps; } inline bool isEqualFuzzy(const mpreal& a, const mpreal& b) { - return isEqualFuzzy(a,b,machine_epsilon((std::min)(abs(a), abs(b)))); + return isEqualFuzzy(a,b,machine_epsilon((std::min)(abs(a), abs(b)))); } inline const mpreal modf(const mpreal& v, mpreal& n) { - mpreal frac(v); + mpreal frac(v); - // rounding is not important since we are using the same number - mpfr_frac(frac.mp,frac.mp,mpreal::default_rnd); - mpfr_trunc(n.mp,v.mp); - return frac; + // rounding is not important since we are using the same number + mpfr_frac(frac.mp,frac.mp,mpreal::default_rnd); + mpfr_trunc(n.mp,v.mp); + return frac; } inline int mpreal::check_range (int t, mp_rnd_t rnd_mode) { - return mpfr_check_range(mp,t,rnd_mode); + return mpfr_check_range(mp,t,rnd_mode); } inline int mpreal::subnormalize (int t,mp_rnd_t rnd_mode) { - int r = mpfr_subnormalize(mp,t,rnd_mode); - MPREAL_MSVC_DEBUGVIEW_CODE; - return r; + int r = mpfr_subnormalize(mp,t,rnd_mode); + MPREAL_MSVC_DEBUGVIEW_CODE; + return r; } inline mp_exp_t mpreal::get_emin (void) { - return mpfr_get_emin(); + return mpfr_get_emin(); } inline int mpreal::set_emin (mp_exp_t exp) { - return mpfr_set_emin(exp); + return mpfr_set_emin(exp); } inline mp_exp_t mpreal::get_emax (void) { - return mpfr_get_emax(); + return mpfr_get_emax(); } inline int mpreal::set_emax (mp_exp_t exp) { - return mpfr_set_emax(exp); + return mpfr_set_emax(exp); } inline mp_exp_t mpreal::get_emin_min (void) { - return mpfr_get_emin_min(); + return mpfr_get_emin_min(); } inline mp_exp_t mpreal::get_emin_max (void) { - return mpfr_get_emin_max(); + return mpfr_get_emin_max(); } inline mp_exp_t mpreal::get_emax_min (void) { - return mpfr_get_emax_min(); + return mpfr_get_emax_min(); } inline mp_exp_t mpreal::get_emax_max (void) { - return mpfr_get_emax_max(); + return mpfr_get_emax_max(); } ////////////////////////////////////////////////////////////////////////// @@ -1637,460 +1637,460 @@ inline mp_exp_t mpreal::get_emax_max (void) ////////////////////////////////////////////////////////////////////////// inline const mpreal sqr(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sqr(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sqr(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal sqrt(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sqrt(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sqrt(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal sqrt(const unsigned long int v, mp_rnd_t rnd_mode) { - mpreal x; - mpfr_sqrt_ui(x.mp,v,rnd_mode); - return x; + mpreal x; + mpfr_sqrt_ui(x.mp,v,rnd_mode); + return x; } inline const mpreal sqrt(const unsigned int v, mp_rnd_t rnd_mode) { - return sqrt(static_cast(v),rnd_mode); + return sqrt(static_cast(v),rnd_mode); } inline const mpreal sqrt(const long int v, mp_rnd_t rnd_mode) { - if (v>=0) return sqrt(static_cast(v),rnd_mode); - else return mpreal().setNan(); // NaN + if (v>=0) return sqrt(static_cast(v),rnd_mode); + else return mpreal().setNan(); // NaN } inline const mpreal sqrt(const int v, mp_rnd_t rnd_mode) { - if (v>=0) return sqrt(static_cast(v),rnd_mode); - else return mpreal().setNan(); // NaN + if (v>=0) return sqrt(static_cast(v),rnd_mode); + else return mpreal().setNan(); // NaN } inline const mpreal sqrt(const long double v, mp_rnd_t rnd_mode) { - return sqrt(mpreal(v),rnd_mode); + return sqrt(mpreal(v),rnd_mode); } inline const mpreal sqrt(const double v, mp_rnd_t rnd_mode) { - return sqrt(mpreal(v),rnd_mode); + return sqrt(mpreal(v),rnd_mode); } inline const mpreal cbrt(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cbrt(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cbrt(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal root(const mpreal& v, unsigned long int k, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_root(x.mp,x.mp,k,rnd_mode); - return x; + mpreal x(v); + mpfr_root(x.mp,x.mp,k,rnd_mode); + return x; } inline const mpreal fabs(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_abs(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_abs(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal abs(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_abs(x.mp,x.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_abs(x.mp,x.mp,rnd_mode); + return x; } inline const mpreal dim(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_dim(x.mp,a.mp,b.mp,rnd_mode); - return x; + mpreal x(a); + mpfr_dim(x.mp,a.mp,b.mp,rnd_mode); + return x; } inline int cmpabs(const mpreal& a,const mpreal& b) { - return mpfr_cmpabs(a.mp,b.mp); + return mpfr_cmpabs(a.mp,b.mp); } inline const mpreal log (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal log2(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log2(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log2(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal log10(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log10(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log10(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal exp(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_exp(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_exp(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal exp2(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_exp2(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_exp2(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal exp10(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_exp10(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_exp10(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal cos(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cos(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cos(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sin(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sin(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sin(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal tan(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_tan(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_tan(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sec(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sec(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sec(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal csc(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_csc(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_csc(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal cot(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cot(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cot(x.mp,v.mp,rnd_mode); + return x; } inline int sin_cos(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) { - return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sin_cos(s.mp,c.mp,v.mp,rnd_mode); } inline const mpreal acos (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_acos(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_acos(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal asin (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_asin(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_asin(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal atan (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_atan(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_atan(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal acot (const mpreal& v, mp_rnd_t rnd_mode) { - return atan(1/v, rnd_mode); + return atan(1/v, rnd_mode); } inline const mpreal asec (const mpreal& v, mp_rnd_t rnd_mode) { - return acos(1/v, rnd_mode); + return acos(1/v, rnd_mode); } inline const mpreal acsc (const mpreal& v, mp_rnd_t rnd_mode) { - return asin(1/v, rnd_mode); + return asin(1/v, rnd_mode); } inline const mpreal acoth (const mpreal& v, mp_rnd_t rnd_mode) { - return atanh(1/v, rnd_mode); + return atanh(1/v, rnd_mode); } inline const mpreal asech (const mpreal& v, mp_rnd_t rnd_mode) { - return acosh(1/v, rnd_mode); + return acosh(1/v, rnd_mode); } inline const mpreal acsch (const mpreal& v, mp_rnd_t rnd_mode) { - return asinh(1/v, rnd_mode); + return asinh(1/v, rnd_mode); } inline const mpreal atan2 (const mpreal& y, const mpreal& x, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t yp, xp; + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); + mpfr_atan2(a.mp, y.mp, x.mp, rnd_mode); - return a; + return a; } inline const mpreal cosh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_cosh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_cosh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sinh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sinh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sinh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal tanh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_tanh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_tanh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal sech (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_sech(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_sech(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal csch (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_csch(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_csch(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal coth (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_coth(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_coth(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal acosh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_acosh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_acosh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal asinh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_asinh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_asinh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal atanh (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_atanh(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_atanh(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal hypot (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t yp, xp; + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); + mpfr_hypot(a.mp, x.mp, y.mp, rnd_mode); - return a; + return a; } inline const mpreal remainder (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) -{ - mpreal a; - mp_prec_t yp, xp; +{ + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); + mpfr_remainder(a.mp, x.mp, y.mp, rnd_mode); - return a; + return a; } inline const mpreal fac_ui (unsigned long int v, mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x(0,prec); - mpfr_fac_ui(x.mp,v,rnd_mode); - return x; + mpreal x(0,prec); + mpfr_fac_ui(x.mp,v,rnd_mode); + return x; } inline const mpreal log1p (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_log1p(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_log1p(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal expm1 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_expm1(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_expm1(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal eint (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_eint(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_eint(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal gamma (const mpreal& x, mp_rnd_t rnd_mode) { - mpreal FunctionValue(x); + mpreal FunctionValue(x); - // x < 0: gamma(-x) = -pi/(x * gamma(x) * sin(pi*x)) + // x < 0: gamma(-x) = -pi/(x * gamma(x) * sin(pi*x)) - mpfr_gamma(FunctionValue.mp, x.mp, rnd_mode); + mpfr_gamma(FunctionValue.mp, x.mp, rnd_mode); - return FunctionValue; + return FunctionValue; } inline const mpreal lngamma (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_lngamma(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_lngamma(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal lgamma (const mpreal& v, int *signp, mp_rnd_t rnd_mode) { - mpreal x(v); - int tsignp; + mpreal x(v); + int tsignp; - if(signp) - mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); - else - mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); + if(signp) + mpfr_lgamma(x.mp,signp,v.mp,rnd_mode); + else + mpfr_lgamma(x.mp,&tsignp,v.mp,rnd_mode); - return x; + return x; } inline const mpreal zeta (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_zeta(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_zeta(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal erf (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_erf(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_erf(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal erfc (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_erfc(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_erfc(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besselj0 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_j0(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_j0(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besselj1 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_j1(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_j1(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besseljn (long n, const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_jn(x.mp,n,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_jn(x.mp,n,v.mp,rnd_mode); + return x; } inline const mpreal bessely0 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_y0(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_y0(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal bessely1 (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_y1(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_y1(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_yn(x.mp,n,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_yn(x.mp,n,v.mp,rnd_mode); + return x; } ////////////////////////////////////////////////////////////////////////// @@ -2099,36 +2099,36 @@ inline const mpreal besselyn (long n, const mpreal& v, mp_rnd_t rnd_mode) inline int sinh_cosh(mpreal& s, mpreal& c, const mpreal& v, mp_rnd_t rnd_mode) { - return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); + return mpfr_sinh_cosh(s.mp,c.mp,v.mp,rnd_mode); } inline const mpreal li2(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_li2(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_li2(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal fmod (const mpreal& x, const mpreal& y, mp_rnd_t rnd_mode) { - mpreal a; - mp_prec_t yp, xp; + mpreal a; + mp_prec_t yp, xp; - yp = y.get_prec(); - xp = x.get_prec(); + yp = y.get_prec(); + xp = x.get_prec(); - a.set_prec(yp>xp?yp:xp); + a.set_prec(yp>xp?yp:xp); - mpfr_fmod(a.mp, x.mp, y.mp, rnd_mode); + mpfr_fmod(a.mp, x.mp, y.mp, rnd_mode); - return a; + return a; } inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rec_sqrt(x.mp,v.mp,rnd_mode); + return x; } #endif // MPFR 2.4.0 Specifics @@ -2138,16 +2138,16 @@ inline const mpreal rec_sqrt(const mpreal& v, mp_rnd_t rnd_mode) inline const mpreal digamma(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_digamma(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_digamma(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_ai(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_ai(x.mp,v.mp,rnd_mode); + return x; } #endif // MPFR 3.0.0 Specifics @@ -2156,192 +2156,192 @@ inline const mpreal ai(const mpreal& v, mp_rnd_t rnd_mode) // Constants inline const mpreal const_log2 (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_log2(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_log2(x.mp,rnd_mode); + return x; } inline const mpreal const_pi (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_pi(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_pi(x.mp,rnd_mode); + return x; } inline const mpreal const_euler (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_euler(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_euler(x.mp,rnd_mode); + return x; } inline const mpreal const_catalan (mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec); - mpfr_const_catalan(x.mp,rnd_mode); - return x; + mpreal x; + x.set_prec(prec); + mpfr_const_catalan(x.mp,rnd_mode); + return x; } inline const mpreal const_infinity (int sign, mp_prec_t prec, mp_rnd_t rnd_mode) { - mpreal x; - x.set_prec(prec,rnd_mode); - mpfr_set_inf(x.mp, sign); - return x; + mpreal x; + x.set_prec(prec,rnd_mode); + mpfr_set_inf(x.mp, sign); + return x; } ////////////////////////////////////////////////////////////////////////// // Integer Related Functions inline const mpreal rint(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal ceil(const mpreal& v) { - mpreal x(v); - mpfr_ceil(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_ceil(x.mp,v.mp); + return x; } inline const mpreal floor(const mpreal& v) { - mpreal x(v); - mpfr_floor(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_floor(x.mp,v.mp); + return x; } inline const mpreal round(const mpreal& v) { - mpreal x(v); - mpfr_round(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_round(x.mp,v.mp); + return x; } inline const mpreal trunc(const mpreal& v) { - mpreal x(v); - mpfr_trunc(x.mp,v.mp); - return x; + mpreal x(v); + mpfr_trunc(x.mp,v.mp); + return x; } inline const mpreal rint_ceil (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_ceil(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_ceil(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal rint_floor(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_floor(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_floor(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal rint_round(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_round(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_round(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal rint_trunc(const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_rint_trunc(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_rint_trunc(x.mp,v.mp,rnd_mode); + return x; } inline const mpreal frac (const mpreal& v, mp_rnd_t rnd_mode) { - mpreal x(v); - mpfr_frac(x.mp,v.mp,rnd_mode); - return x; + mpreal x(v); + mpfr_frac(x.mp,v.mp,rnd_mode); + return x; } ////////////////////////////////////////////////////////////////////////// // Miscellaneous Functions inline void swap(mpreal& a, mpreal& b) { - mpfr_swap(a.mp,b.mp); + mpfr_swap(a.mp,b.mp); } inline const mpreal (max)(const mpreal& x, const mpreal& y) { - return (x>y?x:y); + return (x>y?x:y); } inline const mpreal (min)(const mpreal& x, const mpreal& y) { - return (x= MPFR_VERSION_NUM(3,0,0)) // use gmp_randinit_default() to init state, gmp_randclear() to clear inline const mpreal urandom (gmp_randstate_t& state, mp_rnd_t rnd_mode) { - mpreal x; - mpfr_urandom(x.mp,state,rnd_mode); - return x; + mpreal x; + mpfr_urandom(x.mp,state,rnd_mode); + return x; } #endif #if (MPFR_VERSION <= MPFR_VERSION_NUM(2,4,2)) inline const mpreal random2 (mp_size_t size, mp_exp_t exp) { - mpreal x; - mpfr_random2(x.mp,size,exp); - return x; + mpreal x; + mpfr_random2(x.mp,size,exp); + return x; } #endif @@ -2353,22 +2353,22 @@ inline const mpreal random(unsigned int seed) { #if (MPFR_VERSION >= MPFR_VERSION_NUM(3,0,0)) - static gmp_randstate_t state; - static bool isFirstTime = true; + static gmp_randstate_t state; + static bool isFirstTime = true; - if(isFirstTime) - { - gmp_randinit_default(state); - gmp_randseed_ui(state,0); - isFirstTime = false; - } + if(isFirstTime) + { + gmp_randinit_default(state); + gmp_randseed_ui(state,0); + isFirstTime = false; + } - if(seed != 0) gmp_randseed_ui(state,seed); + if(seed != 0) gmp_randseed_ui(state,seed); - return mpfr::urandom(state); + return mpfr::urandom(state); #else - if(seed != 0) std::srand(seed); - return mpfr::mpreal(std::rand()/(double)RAND_MAX); + if(seed != 0) std::srand(seed); + return mpfr::mpreal(std::rand()/(double)RAND_MAX); #endif } @@ -2377,346 +2377,346 @@ inline const mpreal random(unsigned int seed) // Set/Get global properties inline void mpreal::set_default_prec(mp_prec_t prec) { - default_prec = prec; - mpfr_set_default_prec(prec); + default_prec = prec; + mpfr_set_default_prec(prec); } inline mp_prec_t mpreal::get_default_prec() { - return (mpfr_get_default_prec)(); + return (mpfr_get_default_prec)(); } inline void mpreal::set_default_base(int base) { - default_base = base; + default_base = base; } inline int mpreal::get_default_base() { - return default_base; + return default_base; } inline void mpreal::set_default_rnd(mp_rnd_t rnd_mode) { - default_rnd = rnd_mode; - mpfr_set_default_rounding_mode(rnd_mode); + default_rnd = rnd_mode; + mpfr_set_default_rounding_mode(rnd_mode); } inline mp_rnd_t mpreal::get_default_rnd() { - return static_cast((mpfr_get_default_rounding_mode)()); + return static_cast((mpfr_get_default_rounding_mode)()); } inline void mpreal::set_double_bits(int dbits) { - double_bits = dbits; + double_bits = dbits; } inline int mpreal::get_double_bits() { - return double_bits; + return double_bits; } inline bool mpreal::fits_in_bits(double x, int n) { - int i; - double t; - return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); + int i; + double t; + return IsInf(x) || (std::modf ( std::ldexp ( std::frexp ( x, &i ), n ), &t ) == 0.0); } inline const mpreal pow(const mpreal& a, const mpreal& b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); - return x; + mpreal x(a); + mpfr_pow(x.mp,x.mp,b.mp,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const mpz_t b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow_z(x.mp,x.mp,b,rnd_mode); - return x; + mpreal x(a); + mpfr_pow_z(x.mp,x.mp,b,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const unsigned long int b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); - return x; + mpreal x(a); + mpfr_pow_ui(x.mp,x.mp,b,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(a,static_cast(b),rnd_mode); + return pow(a,static_cast(b),rnd_mode); } inline const mpreal pow(const mpreal& a, const long int b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_pow_si(x.mp,x.mp,b,rnd_mode); - return x; + mpreal x(a); + mpfr_pow_si(x.mp,x.mp,b,rnd_mode); + return x; } inline const mpreal pow(const mpreal& a, const int b, mp_rnd_t rnd_mode) { - return pow(a,static_cast(b),rnd_mode); + return pow(a,static_cast(b),rnd_mode); } inline const mpreal pow(const mpreal& a, const long double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); + return pow(a,mpreal(b),rnd_mode); } inline const mpreal pow(const mpreal& a, const double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); + return pow(a,mpreal(b),rnd_mode); } inline const mpreal pow(const unsigned long int a, const mpreal& b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); - return x; + mpreal x(a); + mpfr_ui_pow(x.mp,a,b.mp,rnd_mode); + return x; } inline const mpreal pow(const unsigned int a, const mpreal& b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),b,rnd_mode); + return pow(static_cast(a),b,rnd_mode); } inline const mpreal pow(const long int a, const mpreal& b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + if (a>=0) return pow(static_cast(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const int a, const mpreal& b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),b,rnd_mode); - else return pow(mpreal(a),b,rnd_mode); + if (a>=0) return pow(static_cast(a),b,rnd_mode); + else return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const long double a, const mpreal& b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); + return pow(mpreal(a),b,rnd_mode); } inline const mpreal pow(const double a, const mpreal& b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); + return pow(mpreal(a),b,rnd_mode); } // pow unsigned long int inline const mpreal pow(const unsigned long int a, const unsigned long int b, mp_rnd_t rnd_mode) { - mpreal x(a); - mpfr_ui_pow_ui(x.mp,a,b,rnd_mode); - return x; + mpreal x(a); + mpfr_ui_pow_ui(x.mp,a,b,rnd_mode); + return x; } inline const mpreal pow(const unsigned long int a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui + return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui } inline const mpreal pow(const unsigned long int a, const long int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(a,static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const long double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned long int a, const double b, mp_rnd_t rnd_mode) { - return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(a,mpreal(b),rnd_mode); //mpfr_ui_pow } // pow unsigned int inline const mpreal pow(const unsigned int a, const unsigned long int b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui + return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui } inline const mpreal pow(const unsigned int a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui } inline const mpreal pow(const unsigned int a, const long int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned int a, const int b, mp_rnd_t rnd_mode) { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned int a, const long double b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } inline const mpreal pow(const unsigned int a, const double b, mp_rnd_t rnd_mode) { - return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow } // pow long int inline const mpreal pow(const long int a, const unsigned long int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long int a, const unsigned int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long int a, const long int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const long int a, const int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const long int a, const long double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } inline const mpreal pow(const long int a, const double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } // pow int inline const mpreal pow(const int a, const unsigned long int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),b,rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const int a, const unsigned int b, mp_rnd_t rnd_mode) { - if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui + if (a>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const int a, const long int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const int a, const int b, mp_rnd_t rnd_mode) { - if (a>0) - { - if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui - else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - }else{ - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si - } + if (a>0) + { + if(b>0) return pow(static_cast(a),static_cast(b),rnd_mode); //mpfr_ui_pow_ui + else return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + }else{ + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + } } inline const mpreal pow(const int a, const long double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } inline const mpreal pow(const int a, const double b, mp_rnd_t rnd_mode) { - if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow - else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow + if (a>=0) return pow(static_cast(a),mpreal(b),rnd_mode); //mpfr_ui_pow + else return pow(mpreal(a),mpreal(b),rnd_mode); //mpfr_pow } // pow long double inline const mpreal pow(const long double a, const long double b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),mpreal(b),rnd_mode); + return pow(mpreal(a),mpreal(b),rnd_mode); } inline const mpreal pow(const long double a, const unsigned long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui + return pow(mpreal(a),b,rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long double a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui + return pow(mpreal(a),static_cast(b),rnd_mode); //mpfr_pow_ui } inline const mpreal pow(const long double a, const long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si } inline const mpreal pow(const long double a, const int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si } inline const mpreal pow(const double a, const double b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),mpreal(b),rnd_mode); + return pow(mpreal(a),mpreal(b),rnd_mode); } inline const mpreal pow(const double a, const unsigned long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_ui + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_ui } inline const mpreal pow(const double a, const unsigned int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_ui + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_ui } inline const mpreal pow(const double a, const long int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si + return pow(mpreal(a),b,rnd_mode); // mpfr_pow_si } inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) { - return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si + return pow(mpreal(a),static_cast(b),rnd_mode); // mpfr_pow_si } } // End of mpfr namespace @@ -2725,11 +2725,11 @@ inline const mpreal pow(const double a, const int b, mp_rnd_t rnd_mode) // Non-throwing swap C++ idiom: http://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Non-throwing_swap namespace std { - template <> - inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) - { - return mpfr::swap(x, y); - } + template <> + inline void swap(mpfr::mpreal& x, mpfr::mpreal& y) + { + return mpfr::swap(x, y); + } } #endif /* __MPREAL_H__ */ diff --git a/gtsam/3rdparty/UFconfig/UFconfig.h b/gtsam/3rdparty/UFconfig/UFconfig.h index 464867b33..795f5668c 100644 --- a/gtsam/3rdparty/UFconfig/UFconfig.h +++ b/gtsam/3rdparty/UFconfig/UFconfig.h @@ -75,10 +75,10 @@ extern "C" { typedef struct UFconfig_struct { - void *(*malloc_memory) (size_t) ; /* pointer to malloc */ + void *(*malloc_memory) (size_t) ; /* pointer to malloc */ void *(*realloc_memory) (void *, size_t) ; /* pointer to realloc */ - void (*free_memory) (void *) ; /* pointer to free */ - void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ + void (*free_memory) (void *) ; /* pointer to free */ + void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ } UFconfig ; @@ -111,31 +111,31 @@ void *UFfree /* always returns NULL */ * * SuiteSparse Version 3.6.1 contains the following packages: * - * AMD version 2.2.2 - * BTF version 1.1.2 - * CAMD version 2.2.2 - * CCOLAMD version 2.7.3 - * CHOLMOD version 1.7.3 - * COLAMD version 2.7.3 - * CSparse version 2.2.5 - * CSparse3 version 3.0.1 - * CXSparse version 2.2.5 - * KLU version 1.1.2 - * LDL version 2.0.3 - * RBio version 2.0.1 + * AMD version 2.2.2 + * BTF version 1.1.2 + * CAMD version 2.2.2 + * CCOLAMD version 2.7.3 + * CHOLMOD version 1.7.3 + * COLAMD version 2.7.3 + * CSparse version 2.2.5 + * CSparse3 version 3.0.1 + * CXSparse version 2.2.5 + * KLU version 1.1.2 + * LDL version 2.0.3 + * RBio version 2.0.1 * SPQR version 1.2.2 (also called SuiteSparseQR) * UFcollection version 1.5.0 - * UFconfig version number is the same as SuiteSparse - * UMFPACK version 5.5.1 + * UFconfig version number is the same as SuiteSparse + * UMFPACK version 5.5.1 * LINFACTOR version 1.1.0 * MESHND version 1.1.1 * SSMULT version 2.0.2 * MATLAB_Tools no specific version number * * Other package dependencies: - * BLAS required by CHOLMOD and UMFPACK - * LAPACK required by CHOLMOD - * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) + * BLAS required by CHOLMOD and UMFPACK + * LAPACK required by CHOLMOD + * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) */ #define SUITESPARSE_DATE "May 10, 2011" diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index 6d79dcb53..2bf18916d 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -26,71 +26,71 @@ namespace gtsam { /* ************************************************************************* */ DSFVector::DSFVector (const size_t numNodes) { - v_ = boost::make_shared(numNodes); - int index = 0; - keys_.reserve(numNodes); - for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) { - *it = index; - keys_.push_back(index); - } + v_ = boost::make_shared(numNodes); + int index = 0; + keys_.reserve(numNodes); + for(V::iterator it = v_->begin(); it!=v_->end(); it++, index++) { + *it = index; + keys_.push_back(index); + } } /* ************************************************************************* */ DSFVector::DSFVector(const boost::shared_ptr& v_in, const std::vector& keys) : keys_(keys) { - v_ = v_in; - BOOST_FOREACH(const size_t key, keys) - (*v_)[key] = key; + v_ = v_in; + BOOST_FOREACH(const size_t key, keys) + (*v_)[key] = key; } /* ************************************************************************* */ bool DSFVector::isSingleton(const Label& label) const { - bool result = false; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); ++it) { - if(findSet(*it) == label) { - if (!result) // find the first occurrence - result = true; - else - return false; - } - } - return result; + bool result = false; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); ++it) { + if(findSet(*it) == label) { + if (!result) // find the first occurrence + result = true; + else + return false; + } + } + return result; } /* ************************************************************************* */ std::set DSFVector::set(const Label& label) const { - std::set set; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - if (findSet(*it) == label) - set.insert(*it); - } - return set; + std::set set; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); it++) { + if (findSet(*it) == label) + set.insert(*it); + } + return set; } /* ************************************************************************* */ std::map > DSFVector::sets() const { - std::map > sets; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - sets[findSet(*it)].insert(*it); - } - return sets; + std::map > sets; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); it++) { + sets[findSet(*it)].insert(*it); + } + return sets; } /* ************************************************************************* */ std::map > DSFVector::arrays() const { - std::map > arrays; - V::const_iterator it = keys_.begin(); - for (; it != keys_.end(); it++) { - arrays[findSet(*it)].push_back(*it); - } - return arrays; + std::map > arrays; + V::const_iterator it = keys_.begin(); + for (; it != keys_.end(); it++) { + arrays[findSet(*it)].push_back(*it); + } + return arrays; } /* ************************************************************************* */ void DSFVector::makeUnionInPlace(const size_t& i1, const size_t& i2) { - (*v_)[findSet(i2)] = findSet(i1); + (*v_)[findSet(i2)] = findSet(i1); } } // namespace gtsam diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index 3940723c4..80b26c3ef 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -25,55 +25,55 @@ namespace gtsam { - /** - * A fast implementation of disjoint set forests that uses vector as underly data structure. - * @addtogroup base - */ - class DSFVector { + /** + * A fast implementation of disjoint set forests that uses vector as underly data structure. + * @addtogroup base + */ + class DSFVector { - public: - typedef std::vector V; ///< Vector of ints - typedef size_t Label; ///< Label type - typedef V::const_iterator const_iterator; ///< const iterator over V - typedef V::iterator iterator;///< iterator over V + public: + typedef std::vector V; ///< Vector of ints + typedef size_t Label; ///< Label type + typedef V::const_iterator const_iterator; ///< const iterator over V + typedef V::iterator iterator;///< iterator over V - private: - // TODO could use existing memory to improve the efficiency - boost::shared_ptr v_; - std::vector keys_; + private: + // TODO could use existing memory to improve the efficiency + boost::shared_ptr v_; + std::vector keys_; - public: - /// constructor that allocate a new memory - DSFVector(const size_t numNodes); + public: + /// constructor that allocate a new memory + DSFVector(const size_t numNodes); - /// constructor that uses the existing memory - DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); + /// constructor that uses the existing memory + DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); - /// find the label of the set in which {key} lives - inline Label findSet(size_t key) const { - size_t parent = (*v_)[key]; - while (parent != key) { - key = parent; - parent = (*v_)[key]; - } - return parent; - } + /// find the label of the set in which {key} lives + inline Label findSet(size_t key) const { + size_t parent = (*v_)[key]; + while (parent != key) { + key = parent; + parent = (*v_)[key]; + } + return parent; + } - /// find whether there is one and only one occurrence for the given {label} - bool isSingleton(const Label& label) const; + /// find whether there is one and only one occurrence for the given {label} + bool isSingleton(const Label& label) const; - /// get the nodes in the tree with the given label - std::set set(const Label& label) const; + /// get the nodes in the tree with the given label + std::set set(const Label& label) const; - /// return all sets, i.e. a partition of all elements - std::map > sets() const; + /// return all sets, i.e. a partition of all elements + std::map > sets() const; - /// return all sets, i.e. a partition of all elements - std::map > arrays() const; + /// return all sets, i.e. a partition of all elements + std::map > arrays() const; - /// the in-place version of makeUnion - void makeUnionInPlace(const size_t& i1, const size_t& i2); + /// the in-place version of makeUnion + void makeUnionInPlace(const size_t& i1, const size_t& i2); - }; + }; } diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 2dc6f3d67..9c61667ff 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -26,21 +26,21 @@ template class DerivedValue : public Value { protected: - DerivedValue() {} + DerivedValue() {} public: - virtual ~DerivedValue() {} + virtual ~DerivedValue() {} /** * Create a duplicate object returned as a pointer to the generic Value interface. * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. + * The result must be deleted with Value::deallocate_, not with the 'delete' operator. */ virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - DERIVED* ptr = new(place) DERIVED(static_cast(*this)); - return ptr; + void *place = boost::singleton_pool::malloc(); + DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + return ptr; } /** @@ -48,15 +48,15 @@ public: */ virtual void deallocate_() const { this->~DerivedValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*)this); // Release memory from pool + boost::singleton_pool::free((void*)this); // Release memory from pool } - /** - * Clone this value (normal clone on the heap, delete with 'delete' operator) - */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(static_cast(*this)); - } + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(static_cast(*this)); + } /// equals implementing generic Value interface virtual bool equals_(const Value& p, double tol = 1e-9) const { @@ -68,7 +68,7 @@ public: } /// Generic Value interface version of retract - virtual Value* retract_(const Vector& delta) const { + virtual Value* retract_(const Vector& delta) const { // Call retract on the derived class const DERIVED retractResult = (static_cast(this))->retract(delta); @@ -78,30 +78,30 @@ public: // Return the pointer to the Value base class return resultAsValue; - } + } - /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { // Cast the base class Value pointer to a derived class pointer const DERIVED& derivedValue2 = dynamic_cast(value2); // Return the result of calling localCoordinates on the derived class return (static_cast(this))->localCoordinates(derivedValue2); - } + } - /// Assignment operator - virtual Value& operator=(const Value& rhs) { + /// Assignment operator + virtual Value& operator=(const Value& rhs) { // Cast the base class Value pointer to a derived class pointer const DERIVED& derivedRhs = dynamic_cast(rhs); // Do the assignment and return the result return (static_cast(this))->operator=(derivedRhs); - } + } - /// Conversion to the derived class - operator const DERIVED& () const { - return static_cast(*this); - } + /// Conversion to the derived class + operator const DERIVED& () const { + return static_cast(*this); + } /// Conversion to the derived class operator DERIVED& () { @@ -109,16 +109,16 @@ public: } protected: - /// Assignment operator, protected because only the Value or DERIVED - /// assignment operators should be used. - DerivedValue& operator=(const DerivedValue& rhs) { - // Nothing to do, do not call base class assignment operator - return *this; - } + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + DerivedValue& operator=(const DerivedValue& rhs) { + // Nothing to do, do not call base class assignment operator + return *this; + } private: - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { }; + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { }; }; diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index aa1ab24e8..5c80799ce 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -31,7 +31,7 @@ namespace gtsam { * convenience to avoid having lengthy types in the code. Through timing, * we've seen that the fast_pool_allocator can lead to speedups of several * percent. - * @addtogroup base + * @addtogroup base */ template class FastList: public std::list > { diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index 33bc5cb7c..f725cacc0 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -88,10 +88,10 @@ public: Base::assign(x.begin(), x.end()); } - /** Conversion to a standard STL container */ - operator std::vector() const { - return std::vector(this->begin(), this->end()); - } + /** Conversion to a standard STL container */ + operator std::vector() const { + return std::vector(this->begin(), this->end()); + } private: /** Serialization function */ diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 3997b6c30..55762a108 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -28,21 +28,21 @@ namespace gtsam { template class GroupConcept { private: - static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + static T concept_check(const T& t) { + /** assignment */ + T t2 = t; - /** identity */ - T identity = T::identity(); + /** identity */ + T identity = T::identity(); - /** compose with another object */ - T compose_ret = identity.compose(t2); + /** compose with another object */ + T compose_ret = identity.compose(t2); - /** invert the object and yield a new one */ - T inverse_ret = compose_ret.inverse(); + /** invert the object and yield a new one */ + T inverse_ret = compose_ret.inverse(); - return inverse_ret; - } + return inverse_ret; + } }; } // \namespace gtsam diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index a770d9975..7a9d32249 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -60,15 +60,15 @@ inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap * as x2 = x1.compose(Expmap(v)). * * Expmap around identity - * static T Expmap(const Vector& v); + * static T Expmap(const Vector& v); * * Logmap around identity - * static Vector Logmap(const T& p); + * static Vector Logmap(const T& p); * * Compute l0 s.t. l2=l1*l0, where (*this) is l1 * A default implementation of between(*this, lp) is available: * between_default() - * T between(const T& l2) const; + * T between(const T& l2) const; * * By convention, we use capital letters to designate a static function * @@ -77,28 +77,28 @@ inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap template class LieConcept { private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { + /** concept checking function - implement the functions this demands */ + static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); + /** expmap around identity */ + T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); + /** Logmap around identity */ + Vector logmap_identity_ret = T::Logmap(t); - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = expmap_identity_ret.between(t2); + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = expmap_identity_ret.between(t2); - return between_ret; - } + return between_ret; + } }; @@ -112,10 +112,10 @@ private: /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? template T BCH(const T& X, const T& Y) { - static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; - T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; + T X_Y = bracket(X, Y); + return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, + bracket(X, X_Y)); } /** @@ -132,8 +132,8 @@ template Matrix wedge(const Vector& x); */ template T expm(const Vector& x, int K=7) { - Matrix xhat = wedge(x); - return T(expm(xhat,K)); + Matrix xhat = wedge(x); + return T(expm(xhat,K)); } } // namespace gtsam @@ -147,11 +147,11 @@ T expm(const Vector& x, int K=7) { * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; #define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 5844e8201..eb52011a1 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -31,109 +31,109 @@ namespace gtsam { */ struct LieMatrix : public Matrix, public DerivedValue { - /** default constructor - should be unnecessary */ - LieMatrix() {} + /** default constructor - should be unnecessary */ + LieMatrix() {} - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} + /** initialize from a normal matrix */ + LieMatrix(const Matrix& v) : Matrix(v) {} - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} + /** initialize from a fixed size normal vector */ + template + LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : + /** constructor with size and initial data, row order ! */ + LieMatrix(size_t m, size_t n, const double* const data) : Matrix(Eigen::Map(data, m, n)) {} - /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ - LieMatrix(size_t m, size_t n, ...); + /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ + LieMatrix(size_t m, size_t n, ...); - /** get the underlying vector */ - inline Matrix matrix() const { - return static_cast(*this); - } + /** get the underlying vector */ + inline Matrix matrix() const { + return static_cast(*this); + } - /** print @param s optional string naming the object */ - inline void print(const std::string& name="") const { - gtsam::print(matrix(), name); - } + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + gtsam::print(matrix(), name); + } - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } + /** equality up to tolerance */ + inline bool equals(const LieMatrix& expected, double tol=1e-5) const { + return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); + } - // Manifold requirements + // Manifold requirements - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->size(); } + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return this->size(); } - /** Update the LieMatrix with a tangent space update. The elements of the + /** Update the LieMatrix with a tangent space update. The elements of the * tangent space vector correspond to the matrix entries arranged in * *row-major* order. */ - inline LieMatrix retract(const Vector& v) const { + inline LieMatrix retract(const Vector& v) const { if(v.size() != this->rows() * this->cols()) throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); return LieMatrix(*this + Eigen::Map(&v(0), this->cols(), this->rows()).transpose()); } - /** @return the local coordinates of another object. The elements of the + /** @return the local coordinates of another object. The elements of the * tangent space vector correspond to the matrix entries arranged in * *row-major* order. */ - inline Vector localCoordinates(const LieMatrix& t2) const { + inline Vector localCoordinates(const LieMatrix& t2) const { Vector result(this->rows() * this->cols()); Eigen::Map(&result(0), this->cols(), this->rows()).transpose() = (t2 - *this); return result; } - // Group requirements + // Group requirements - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieMatrix identity() { + throw std::runtime_error("LieMatrix::identity(): Don't use this function"); + return LieMatrix(); + } - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieMatrix compose(const LieMatrix& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); + // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments + // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class + // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector + // as the other geometry objects (Point3, Rot3, etc.) have this problem + /** compose with another object */ + inline LieMatrix compose(const LieMatrix& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(p.dim()); - return LieMatrix(*this + p); - } + return LieMatrix(*this + p); + } - /** between operation */ - inline LieMatrix between(const LieMatrix& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieMatrix(l2 - *this); - } + /** between operation */ + inline LieMatrix between(const LieMatrix& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(l2.dim()); + return LieMatrix(l2 - *this); + } - /** invert the object and yield a new one */ - inline LieMatrix inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); + /** invert the object and yield a new one */ + inline LieMatrix inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); - return LieMatrix(-(*this)); - } + return LieMatrix(-(*this)); + } - // Lie functions + // Lie functions - /** Expmap around identity */ - static inline LieMatrix Expmap(const Vector& v) { + /** Expmap around identity */ + static inline LieMatrix Expmap(const Vector& v) { throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); return LieMatrix(v); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieMatrix& p) { + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } private: diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 3721492ec..dc8f29645 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -22,80 +22,80 @@ namespace gtsam { - /** - * LieScalar is a wrapper around double to allow it to be a Lie type - */ - struct LieScalar : public DerivedValue { + /** + * LieScalar is a wrapper around double to allow it to be a Lie type + */ + struct LieScalar : public DerivedValue { - /** default constructor */ - LieScalar() : d_(0.0) {} + /** default constructor */ + LieScalar() : d_(0.0) {} - /** wrap a double */ - explicit LieScalar(double d) : d_(d) {} + /** wrap a double */ + explicit LieScalar(double d) : d_(d) {} - /** access the underlying value */ - double value() const { return d_; } + /** access the underlying value */ + double value() const { return d_; } - /** Automatic conversion to underlying value */ - operator double() const { return d_; } + /** Automatic conversion to underlying value */ + operator double() const { return d_; } - /** print @param name optional string naming the object */ - inline void print(const std::string& name="") const { - std::cout << name << ": " << d_ << std::endl; - } + /** print @param name optional string naming the object */ + inline void print(const std::string& name="") const { + std::cout << name << ": " << d_ << std::endl; + } - /** equality up to tolerance */ - inline bool equals(const LieScalar& expected, double tol=1e-5) const { - return fabs(expected.d_ - d_) <= tol; - } + /** equality up to tolerance */ + inline bool equals(const LieScalar& expected, double tol=1e-5) const { + return fabs(expected.d_ - d_) <= tol; + } - // Manifold requirements + // Manifold requirements - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return 1; } - inline static size_t Dim() { return 1; } + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } - /** Update the LieScalar with a tangent space update */ - inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } + /** Update the LieScalar with a tangent space update */ + inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } - // Group requirements + // Group requirements - /** identity */ - inline static LieScalar identity() { - return LieScalar(); - } + /** identity */ + inline static LieScalar identity() { + return LieScalar(); + } - /** compose with another object */ - inline LieScalar compose(const LieScalar& p) const { - return LieScalar(d_ + p.d_); - } + /** compose with another object */ + inline LieScalar compose(const LieScalar& p) const { + return LieScalar(d_ + p.d_); + } - /** between operation */ - inline LieScalar between(const LieScalar& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(1); - if(H2) *H2 = eye(1); - return LieScalar(l2.value() - value()); - } + /** between operation */ + inline LieScalar between(const LieScalar& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(1); + if(H2) *H2 = eye(1); + return LieScalar(l2.value() - value()); + } - /** invert the object and yield a new one */ - inline LieScalar inverse() const { - return LieScalar(-1.0 * value()); - } + /** invert the object and yield a new one */ + inline LieScalar inverse() const { + return LieScalar(-1.0 * value()); + } - // Lie functions + // Lie functions - /** Expmap around identity */ - static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } + /** Expmap around identity */ + static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } - private: - double d_; - }; + private: + double d_; + }; } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 916730fd3..20c705a23 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -28,96 +28,96 @@ namespace gtsam { */ struct LieVector : public Vector, public DerivedValue { - /** default constructor - should be unnecessary */ - LieVector() {} + /** default constructor - should be unnecessary */ + LieVector() {} - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} + /** initialize from a normal vector */ + LieVector(const Vector& v) : Vector(v) {} - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} + /** initialize from a fixed size normal vector */ + template + LieVector(const Eigen::Matrix& v) : Vector(v) {} - /** wrap a double */ - LieVector(double d) : Vector(Vector_(1, d)) {} + /** wrap a double */ + LieVector(double d) : Vector(Vector_(1, d)) {} - /** constructor with size and initial data, row order ! */ - LieVector(size_t m, const double* const data); + /** constructor with size and initial data, row order ! */ + LieVector(size_t m, const double* const data); - /** Specify arguments directly, as in Vector_() - always force these to be doubles */ - LieVector(size_t m, ...); + /** Specify arguments directly, as in Vector_() - always force these to be doubles */ + LieVector(size_t m, ...); - /** get the underlying vector */ - inline Vector vector() const { - return static_cast(*this); - } + /** get the underlying vector */ + inline Vector vector() const { + return static_cast(*this); + } - /** print @param name optional string naming the object */ - inline void print(const std::string& name="") const { - gtsam::print(vector(), name); - } + /** print @param name optional string naming the object */ + inline void print(const std::string& name="") const { + gtsam::print(vector(), name); + } - /** equality up to tolerance */ - inline bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } + /** equality up to tolerance */ + inline bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } - // Manifold requirements + // Manifold requirements - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->size(); } + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return this->size(); } - /** Update the LieVector with a tangent space update */ - inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } + /** Update the LieVector with a tangent space update */ + inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } - // Group requirements + // Group requirements - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } + /** identity - NOTE: no known size at compile time - so zero length */ + inline static LieVector identity() { + throw std::runtime_error("LieVector::identity(): Don't use this function"); + return LieVector(); + } - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieVector compose(const LieVector& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); + // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments + // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class + // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector + // as the other geometry objects (Point3, Rot3, etc.) have this problem + /** compose with another object */ + inline LieVector compose(const LieVector& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(p.dim()); - return LieVector(vector() + p); - } + return LieVector(vector() + p); + } - /** between operation */ - inline LieVector between(const LieVector& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieVector(l2.vector() - vector()); - } + /** between operation */ + inline LieVector between(const LieVector& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(l2.dim()); + return LieVector(l2.vector() - vector()); + } - /** invert the object and yield a new one */ - inline LieVector inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); + /** invert the object and yield a new one */ + inline LieVector inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); - return LieVector(-1.0 * vector()); - } + return LieVector(-1.0 * vector()); + } - // Lie functions + // Lie functions - /** Expmap around identity */ - static inline LieVector Expmap(const Vector& v) { return LieVector(v); } + /** Expmap around identity */ + static inline LieVector Expmap(const Vector& v) { return LieVector(v); } - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const LieVector& p) { return p; } + /** Logmap around identity - just returns with default cast back */ + static inline Vector Logmap(const LieVector& p) { return p; } private: diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index b84a6b525..ceebf6bad 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -49,14 +49,14 @@ namespace gtsam { * * Returns dimensionality of the tangent space, which may be smaller than the number * of nonlinear parameters. - * size_t dim() const; + * size_t dim() const; * * Returns a new T that is a result of updating *this with the delta v after pulling * the updated value back to the manifold T. - * T retract(const Vector& v) const; + * T retract(const Vector& v) const; * * Returns the linear coordinates of lp in the tangent space centered around *this. - * Vector localCoordinates(const T& lp) const; + * Vector localCoordinates(const T& lp) const; * * By convention, we use capital letters to designate a static function * @tparam T is a Lie type, like Point2, Pose3, etc. @@ -64,29 +64,29 @@ namespace gtsam { template class ManifoldConcept { private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { + /** concept checking function - implement the functions this demands */ + static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); + /** + * Returns local coordinates of another object + */ + Vector localCoords_ret = t.localCoordinates(t2); - return retract_ret; - } + return retract_ret; + } }; } // namespace gtsam diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1369f13b8..40731591f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -40,200 +40,200 @@ namespace gtsam { /* ************************************************************************* */ Matrix Matrix_( size_t m, size_t n, const double* const data) { - MatrixRowMajor A(m,n); - copy(data, data+m*n, A.data()); - return Matrix(A); + MatrixRowMajor A(m,n); + copy(data, data+m*n, A.data()); + return Matrix(A); } /* ************************************************************************* */ Matrix Matrix_( size_t m, size_t n, const Vector& v) { - Matrix A(m,n); - // column-wise copy - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - A(i,j) = v(k); - return A; + Matrix A(m,n); + // column-wise copy + for( size_t j = 0, k=0 ; j < n ; j++) + for( size_t i = 0; i < m ; i++,k++) + A(i,j) = v(k); + return A; } /* ************************************************************************* */ Matrix Matrix_(size_t m, size_t n, ...) { - Matrix A(m,n); - va_list ap; - va_start(ap, n); - for( size_t i = 0 ; i < m ; i++) - for( size_t j = 0 ; j < n ; j++) { - double value = va_arg(ap, double); - A(i,j) = value; - } - va_end(ap); - return A; + Matrix A(m,n); + va_list ap; + va_start(ap, n); + for( size_t i = 0 ; i < m ; i++) + for( size_t j = 0 ; j < n ; j++) { + double value = va_arg(ap, double); + A(i,j) = value; + } + va_end(ap); + return A; } /* ************************************************************************* */ Matrix zeros( size_t m, size_t n ) { - return Matrix::Zero(m,n); + return Matrix::Zero(m,n); } /* ************************************************************************* */ Matrix eye( size_t m, size_t n) { - return Matrix::Identity(m, n); + return Matrix::Identity(m, n); } /* ************************************************************************* */ Matrix diag(const Vector& v) { - return v.asDiagonal(); + return v.asDiagonal(); } /* ************************************************************************* */ bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; + if (equal_with_abs_tol(expected,actual,tol)) return true; - size_t n1 = expected.cols(), m1 = expected.rows(); - size_t n2 = actual.cols(), m2 = actual.rows(); + size_t n1 = expected.cols(), m1 = expected.rows(); + size_t n2 = actual.cols(), m2 = actual.rows(); - cout << "not equal:" << endl; - print(expected,"expected = "); - print(actual,"actual = "); - if(m1!=m2 || n1!=n2) - cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl; - else { - Matrix diff = actual-expected; - print(diff, "actual - expected = "); - } - return false; + cout << "not equal:" << endl; + print(expected,"expected = "); + print(actual,"actual = "); + if(m1!=m2 || n1!=n2) + cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl; + else { + Matrix diff = actual-expected; + print(diff, "actual - expected = "); + } + return false; } /* ************************************************************************* */ bool assert_equal(const std::list& As, const std::list& Bs, double tol) { - if (As.size() != Bs.size()) return false; + if (As.size() != Bs.size()) return false; - list::const_iterator itA, itB; - itA = As.begin(); itB = Bs.begin(); - for (; itA != As.end(); itA++, itB++) - if (!assert_equal(*itB, *itA, tol)) - return false; + list::const_iterator itA, itB; + itA = As.begin(); itB = Bs.begin(); + for (; itA != As.end(); itA++, itB++) + if (!assert_equal(*itB, *itA, tol)) + return false; - return true; + return true; } /* ************************************************************************* */ static bool is_linear_dependent(const Matrix& A, const Matrix& B, double tol) { - // This local static function is used by linear_independent and - // linear_dependent just below. - size_t n1 = A.cols(), m1 = A.rows(); - size_t n2 = B.cols(), m2 = B.rows(); + // This local static function is used by linear_independent and + // linear_dependent just below. + size_t n1 = A.cols(), m1 = A.rows(); + size_t n2 = B.cols(), m2 = B.rows(); - bool dependent = true; - if(m1!=m2 || n1!=n2) dependent = false; + bool dependent = true; + if(m1!=m2 || n1!=n2) dependent = false; - for(size_t i=0; dependent && i>(istream& inputStream, Matrix& destinationMatrix) { /* ************************************************************************* */ void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) { - fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; + fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; } /* ************************************************************************* */ void insertColumn(Matrix& A, const Vector& col, size_t j) { - A.col(j) = col; + A.col(j) = col; } /* ************************************************************************* */ void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { - A.col(j).segment(i, col.size()) = col; + A.col(j).segment(i, col.size()) = col; } /* ************************************************************************* */ Vector columnNormSquare(const Matrix &A) { - Vector v (A.cols()) ; - for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) { - v[i] = A.col(i).dot(A.col(i)); - } - return v ; + Vector v (A.cols()) ; + for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) { + v[i] = A.col(i).dot(A.col(i)); + } + return v ; } /* ************************************************************************* */ void solve(Matrix& A, Matrix& B) { - // Eigen version - untested - B = A.fullPivLu().solve(B); + // Eigen version - untested + B = A.fullPivLu().solve(B); } /* ************************************************************************* */ Matrix inverse(const Matrix& A) { - return A.inverse(); + return A.inverse(); } /* ************************************************************************* */ /** Householder QR factorization, Golub & Van Loan p 224, explicit version */ /* ************************************************************************* */ pair qr(const Matrix& A) { - const size_t m = A.rows(), n = A.cols(), kprime = min(m,n); + const size_t m = A.rows(), n = A.cols(), kprime = min(m,n); - Matrix Q=eye(m,m),R(A); - Vector v(m); + Matrix Q=eye(m,m),R(A); + Vector v(m); - // loop over the kprime first columns - for(size_t j=0; j < kprime; j++){ + // loop over the kprime first columns + for(size_t j=0; j < kprime; j++){ - // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end) - const size_t mm=m-j; + // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end) + const size_t mm=m-j; - // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j) - Vector xjm(mm); - for(size_t k = 0 ; k < mm; k++) - xjm(k) = R(j+k, j); + // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j) + Vector xjm(mm); + for(size_t k = 0 ; k < mm; k++) + xjm(k) = R(j+k, j); - // calculate the Householder vector v - double beta; Vector vjm; - boost::tie(beta,vjm) = house(xjm); + // calculate the Householder vector v + double beta; Vector vjm; + boost::tie(beta,vjm) = house(xjm); - // pad with zeros to get m-dimensional vector v - for(size_t k = 0 ; k < m; k++) - v(k) = k > weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { - size_t m = A.rows(), n = A.cols(); // get size(A) - size_t maxRank = min(m,n); + size_t m = A.rows(), n = A.cols(); // get size(A) + size_t maxRank = min(m,n); - // create list - list > results; + // create list + list > results; - Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once + Vector pseudo(m); // allocate storage for pseudo-inverse + Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once - // We loop over all columns, because the columns that can be eliminated - // are not necessarily contiguous. For each one, estimate the corresponding - // scalar variable x as d-rS, with S the separator (remaining columns). - // Then update A and b by substituting x with d-rS, zero-ing out x's column. - for (size_t j=0; j=maxRank) break; + // exit after rank exhausted + if (results.size()>=maxRank) break; - // update A, b, expensive, using outer product - // A' \define A_{S}-a*r and b'\define b-d*a - A -= a * r.transpose(); - b -= d * a; - } + // update A, b, expensive, using outer product + // A' \define A_{S}-a*r and b'\define b-d*a + A -= a * r.transpose(); + b -= d * a; + } - return results; + return results; } /* ************************************************************************* */ @@ -400,188 +400,188 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { /* ************************************************************************* */ void householder_(Matrix& A, size_t k, bool copy_vectors) { - const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); - // loop over the kprime first columns - for(size_t j=0; j < kprime; j++) { - // copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j) - Vector vjm = A.col(j).segment(j, m-j); + const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); + // loop over the kprime first columns + for(size_t j=0; j < kprime; j++) { + // copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j) + Vector vjm = A.col(j).segment(j, m-j); - // calculate the Householder vector, in place - double beta = houseInPlace(vjm); + // calculate the Householder vector, in place + double beta = houseInPlace(vjm); - // do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w' - tic(1, "householder_update"); // bottleneck for system - // don't touch old columns - Vector w = beta * A.block(j,j,m-j,n-j).transpose() * vjm; - A.block(j,j,m-j,n-j) -= vjm * w.transpose(); - toc(1, "householder_update"); + // do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w' + tic(1, "householder_update"); // bottleneck for system + // don't touch old columns + Vector w = beta * A.block(j,j,m-j,n-j).transpose() * vjm; + A.block(j,j,m-j,n-j) -= vjm * w.transpose(); + toc(1, "householder_update"); - // the Householder vector is copied in the zeroed out part - if (copy_vectors) { - tic(2, "householder_vector_copy"); - A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1)); - toc(2, "householder_vector_copy"); - } - } // column j + // the Householder vector is copied in the zeroed out part + if (copy_vectors) { + tic(2, "householder_vector_copy"); + A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1)); + toc(2, "householder_vector_copy"); + } + } // column j } /* ************************************************************************* */ void householder(Matrix& A, size_t k) { - // version with zeros below diagonal - tic(1, "householder_"); - householder_(A,k,false); - toc(1, "householder_"); -// tic(2, "householder_zero_fill"); -// const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); -// for(size_t j=0; j < kprime; j++) -// A.col(j).segment(j+1, m-(j+1)).setZero(); -// toc(2, "householder_zero_fill"); + // version with zeros below diagonal + tic(1, "householder_"); + householder_(A,k,false); + toc(1, "householder_"); +// tic(2, "householder_zero_fill"); +// const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n)); +// for(size_t j=0; j < kprime; j++) +// A.col(j).segment(j+1, m-(j+1)).setZero(); +// toc(2, "householder_zero_fill"); } /* ************************************************************************* */ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) { - // @return the solution x of L*x=b - assert(L.rows() == L.cols()); - if (unit) - return L.triangularView().solve(b); - else - return L.triangularView().solve(b); + // @return the solution x of L*x=b + assert(L.rows() == L.cols()); + if (unit) + return L.triangularView().solve(b); + else + return L.triangularView().solve(b); } /* ************************************************************************* */ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) { - // @return the solution x of U*x=b - assert(U.rows() == U.cols()); - if (unit) - return U.triangularView().solve(b); - else - return U.triangularView().solve(b); + // @return the solution x of U*x=b + assert(U.rows() == U.cols()); + if (unit) + return U.triangularView().solve(b); + else + return U.triangularView().solve(b); } /* ************************************************************************* */ Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) { - // @return the solution x of x'*U=b' - assert(U.rows() == U.cols()); - if (unit) - return U.triangularView().transpose().solve(b); - else - return U.triangularView().transpose().solve(b); + // @return the solution x of x'*U=b' + assert(U.rows() == U.cols()); + if (unit) + return U.triangularView().transpose().solve(b); + else + return U.triangularView().transpose().solve(b); } /* ************************************************************************* */ Matrix stack(size_t nrMatrices, ...) { - size_t dimA1 = 0; - size_t dimA2 = 0; - va_list ap; - va_start(ap, nrMatrices); - for(size_t i = 0 ; i < nrMatrices ; i++) { - Matrix *M = va_arg(ap, Matrix *); - dimA1 += M->rows(); - dimA2 = M->cols(); // TODO: should check if all the same ! - } - va_end(ap); - va_start(ap, nrMatrices); - Matrix A(dimA1, dimA2); - size_t vindex = 0; - for( size_t i = 0 ; i < nrMatrices ; i++) { - Matrix *M = va_arg(ap, Matrix *); - for(size_t d1 = 0; d1 < (size_t) M->rows(); d1++) - for(size_t d2 = 0; d2 < (size_t) M->cols(); d2++) - A(vindex+d1, d2) = (*M)(d1, d2); - vindex += M->rows(); - } + size_t dimA1 = 0; + size_t dimA2 = 0; + va_list ap; + va_start(ap, nrMatrices); + for(size_t i = 0 ; i < nrMatrices ; i++) { + Matrix *M = va_arg(ap, Matrix *); + dimA1 += M->rows(); + dimA2 = M->cols(); // TODO: should check if all the same ! + } + va_end(ap); + va_start(ap, nrMatrices); + Matrix A(dimA1, dimA2); + size_t vindex = 0; + for( size_t i = 0 ; i < nrMatrices ; i++) { + Matrix *M = va_arg(ap, Matrix *); + for(size_t d1 = 0; d1 < (size_t) M->rows(); d1++) + for(size_t d2 = 0; d2 < (size_t) M->cols(); d2++) + A(vindex+d1, d2) = (*M)(d1, d2); + vindex += M->rows(); + } - return A; + return A; } /* ************************************************************************* */ Matrix collect(const std::vector& matrices, size_t m, size_t n) { - // if we have known and constant dimensions, use them - size_t dimA1 = m; - size_t dimA2 = n*matrices.size(); - if (!m && !n) { - BOOST_FOREACH(const Matrix* M, matrices) { - dimA1 = M->rows(); // TODO: should check if all the same ! - dimA2 += M->cols(); - } - } + // if we have known and constant dimensions, use them + size_t dimA1 = m; + size_t dimA2 = n*matrices.size(); + if (!m && !n) { + BOOST_FOREACH(const Matrix* M, matrices) { + dimA1 = M->rows(); // TODO: should check if all the same ! + dimA2 += M->cols(); + } + } - // stl::copy version - Matrix A(dimA1, dimA2); - size_t hindex = 0; - BOOST_FOREACH(const Matrix* M, matrices) { - size_t row_len = M->cols(); - A.block(0, hindex, dimA1, row_len) = *M; - hindex += row_len; - } + // stl::copy version + Matrix A(dimA1, dimA2); + size_t hindex = 0; + BOOST_FOREACH(const Matrix* M, matrices) { + size_t row_len = M->cols(); + A.block(0, hindex, dimA1, row_len) = *M; + hindex += row_len; + } - return A; + return A; } /* ************************************************************************* */ Matrix collect(size_t nrMatrices, ...) { - vector matrices; - va_list ap; - va_start(ap, nrMatrices); - for( size_t i = 0 ; i < nrMatrices ; i++) { - Matrix *M = va_arg(ap, Matrix *); - matrices.push_back(M); - } - return collect(matrices); + vector matrices; + va_list ap; + va_start(ap, nrMatrices); + for( size_t i = 0 ; i < nrMatrices ; i++) { + Matrix *M = va_arg(ap, Matrix *); + matrices.push_back(M); + } + return collect(matrices); } /* ************************************************************************* */ // row scaling, in-place void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { - const size_t m = A.rows(); - if (inf_mask) { - for (size_t i=0; i llt; - llt.compute(A); - return llt.matrixL(); + Matrix L = zeros(A.rows(), A.rows()); + Eigen::LLT llt; + llt.compute(A); + return llt.matrixL(); } Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + return LLt(A).transpose(); } /* @@ -627,32 +627,32 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); + // FIXME: replace with real algorithm + return A.inverse(); -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); +// Matrix L = LLt(A); +// Matrix inv(eye(A.rows())); +// inplace_solve (L, inv, BNU::lower_tag ()); +// return BNU::prod(trans(inv), inv); } #if 0 /* ************************************************************************* */ // TODO, would be faster with Cholesky Matrix inverse_square_root(const Matrix& A) { - size_t m = A.cols(), n = A.rows(); - if (m!=n) - throw invalid_argument("inverse_square_root: A must be square"); + size_t m = A.cols(), n = A.rows(); + if (m!=n) + throw invalid_argument("inverse_square_root: A must be square"); - // Perform SVD, TODO: symmetric SVD? - Matrix U,V; - Vector S; - svd(A,U,S,V); + // Perform SVD, TODO: symmetric SVD? + Matrix U,V; + Vector S; + svd(A,U,S,V); - // invert and sqrt diagonal of S - // We also arbitrarily choose sign to make result have positive signs - for(size_t i = 0; i().solveInPlace(inv); - return inv.transpose(); + Matrix R = RtR(A); + Matrix inv = eye(A.rows()); + R.triangularView().solveInPlace(inv); + return inv.transpose(); } /* ************************************************************************* */ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) { - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); - U = svd.matrixU(); - S = svd.singularValues(); - V = svd.matrixV(); + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + U = svd.matrixU(); + S = svd.singularValues(); + V = svd.matrixV(); } /* ************************************************************************* */ boost::tuple DLT(const Matrix& A, double rank_tol) { - // Check size of A - int n = A.rows(), p = A.cols(), m = min(n,p); + // Check size of A + int n = A.rows(), p = A.cols(), m = min(n,p); - // Do SVD on A + // Do SVD on A Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); Vector s = svd.singularValues(); Matrix V = svd.matrixV(); - // Find rank - int rank = 0; - for (int j = 0; j < m; j++) - if (s(j) > rank_tol) rank++; + // Find rank + int rank = 0; + for (int j = 0; j < m; j++) + if (s(j) > rank_tol) rank++; - // Return rank, error, and corresponding column of V - double error = m

(rank, error, Vector(column(V, p-1))); + // Return rank, error, and corresponding column of V + double error = m

(rank, error, Vector(column(V, p-1))); } /* ************************************************************************* */ Matrix expm(const Matrix& A, size_t K) { - Matrix E = eye(A.rows()), A_k = eye(A.rows()); - for(size_t k=1;k<=K;k++) { - A_k = A_k*A/k; - E = E + A_k; - } - return E; + Matrix E = eye(A.rows()), A_k = eye(A.rows()); + for(size_t k=1;k<=K;k++) { + A_k = A_k*A/k; + E = E + A_k; + } + return E; } /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { - int n = A.cols(); - assert(A.rows() == n); + int n = A.cols(); + assert(A.rows() == n); - // original -// const Matrix I = eye(n); -// return (I-A)*inverse(I+A); + // original +// const Matrix I = eye(n); +// return (I-A)*inverse(I+A); - // inlined to let Eigen do more optimization - return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); + // inlined to let Eigen do more optimization + return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index e6a40f727..a20a2357f 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -100,19 +100,19 @@ Matrix diag(const Vector& v); template bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBase& B, double tol = 1e-9) { - const size_t n1 = A.cols(), m1 = A.rows(); - const size_t n2 = B.cols(), m2 = B.rows(); + const size_t n1 = A.cols(), m1 = A.rows(); + const size_t n2 = B.cols(), m2 = B.rows(); - if(m1!=m2 || n1!=n2) return false; + if(m1!=m2 || n1!=n2) return false; - for(size_t i=0; i tol) - return false; - } - return true; + for(size_t i=0; i tol) + return false; + } + return true; } /** @@ -183,8 +183,8 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec /** products using old-style format to improve compatibility */ template inline MATRIX prod(const MATRIX& A, const MATRIX&B) { - MATRIX result = A * B; - return result; + MATRIX result = A * B; + return result; } /** @@ -220,8 +220,8 @@ std::istream& operator>>(std::istream& inputStream, Matrix& destinationMatrix); */ template Eigen::Block sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) { - size_t m=i2-i1, n=j2-j1; - return A.block(i1,j1,m,n); + size_t m=i2-i1, n=j2-j1; + return A.block(i1,j1,m,n); } /** @@ -242,7 +242,7 @@ void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j); */ template const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) { - return A.col(j); + return A.col(j); } /** @@ -253,7 +253,7 @@ const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) { */ template const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { - return A.row(j); + return A.row(j); } /** @@ -276,10 +276,10 @@ Vector columnNormSquare(const Matrix &A); */ template void zeroBelowDiagonal(MATRIX& A, size_t cols=0) { - const size_t m = A.rows(), n = A.cols(); - const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n); - for (size_t j=0; j qr(const Matrix& A); */ template void inplace_QR(MATRIX& A) { - size_t rows = A.rows(); - size_t cols = A.cols(); - size_t size = std::min(rows,cols); + size_t rows = A.rows(); + size_t cols = A.cols(); + size_t size = std::min(rows,cols); - typedef Eigen::internal::plain_diag_type::type HCoeffsType; - typedef Eigen::internal::plain_row_type::type RowVectorType; - HCoeffsType hCoeffs(size); - RowVectorType temp(cols); + typedef Eigen::internal::plain_diag_type::type HCoeffsType; + typedef Eigen::internal::plain_row_type::type RowVectorType; + HCoeffsType hCoeffs(size); + RowVectorType temp(cols); - Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); + Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); - zeroBelowDiagonal(A); + zeroBelowDiagonal(A); } /** @@ -478,8 +478,8 @@ Matrix Cayley(const Matrix& A); /// Eigen do more optimization template Matrix Cayley(const Eigen::Matrix& A) { - typedef Eigen::Matrix FMat; - return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse()); + typedef Eigen::Matrix FMat; + return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse()); } } // namespace gtsam @@ -488,31 +488,31 @@ Matrix Cayley(const Eigen::Matrix& A) { #include namespace boost { - namespace serialization { + namespace serialization { // split version - sends sizes ahead - template - void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { - const int rows = m.rows(), cols = m.cols(), elements = rows * cols; - std::vector raw_data(elements); - std::copy(m.data(), m.data() + elements, raw_data.begin()); - ar << make_nvp("rows", rows); - ar << make_nvp("cols", cols); - ar << make_nvp("data", raw_data); - } + template + void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) { + const int rows = m.rows(), cols = m.cols(), elements = rows * cols; + std::vector raw_data(elements); + std::copy(m.data(), m.data() + elements, raw_data.begin()); + ar << make_nvp("rows", rows); + ar << make_nvp("cols", cols); + ar << make_nvp("data", raw_data); + } - template - void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { - size_t rows, cols; - std::vector raw_data; - ar >> make_nvp("rows", rows); - ar >> make_nvp("cols", cols); - ar >> make_nvp("data", raw_data); - m = gtsam::Matrix(rows, cols); - std::copy(raw_data.begin(), raw_data.end(), m.data()); - } + template + void load(Archive & ar, gtsam::Matrix & m, unsigned int version) { + size_t rows, cols; + std::vector raw_data; + ar >> make_nvp("rows", rows); + ar >> make_nvp("cols", cols); + ar >> make_nvp("data", raw_data); + m = gtsam::Matrix(rows, cols); + std::copy(raw_data.begin(), raw_data.end(), m.data()); + } - } // namespace serialization + } // namespace serialization } // namespace boost BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index b6ae1b3ef..a308c50a1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -20,12 +20,12 @@ * the function exists in derived class and throw compile-time errors. * * print with optional string naming the object - * void print(const std::string& name) const = 0; + * void print(const std::string& name) const = 0; * * equality up to tolerance * tricky to implement, see NoiseModelFactor1 for an example * equals is not supposed to print out *anything*, just return true|false - * bool equals(const Derived& expected, double tol) const = 0; + * bool equals(const Derived& expected, double tol) const = 0; * */ @@ -41,84 +41,84 @@ namespace gtsam { - /** - * A testable concept check that should be placed in applicable unit - * tests and in generic algorithms. - * - * See macros for details on using this structure - * @addtogroup base - * @tparam T is the type this constrains to be testable - assumes print() and equals() - */ - template - class TestableConcept { - static bool checkTestableConcept(const T& d) { - // check print function, with optional string - d.print(std::string()); - d.print(); + /** + * A testable concept check that should be placed in applicable unit + * tests and in generic algorithms. + * + * See macros for details on using this structure + * @addtogroup base + * @tparam T is the type this constrains to be testable - assumes print() and equals() + */ + template + class TestableConcept { + static bool checkTestableConcept(const T& d) { + // check print function, with optional string + d.print(std::string()); + d.print(); - // check print, with optional threshold - double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; - } - }; + // check print, with optional threshold + double tol = 1.0; + bool r1 = d.equals(d, tol); + bool r2 = d.equals(d); + return r1 && r2; + } + }; - /** Call print on the object */ - template - inline void print(const T& object, const std::string& s = "") { - object.print(s); - } + /** Call print on the object */ + template + inline void print(const T& object, const std::string& s = "") { + object.print(s); + } - /** Call equal on the object */ - template - inline bool equal(const T& obj1, const T& obj2, double tol) { - return obj1.equals(obj2, tol); - } + /** Call equal on the object */ + template + inline bool equal(const T& obj1, const T& obj2, double tol) { + return obj1.equals(obj2, tol); + } - /** Call equal on the object without tolerance (use default tolerance) */ - template - inline bool equal(const T& obj1, const T& obj2) { - return obj1.equals(obj2); - } + /** Call equal on the object without tolerance (use default tolerance) */ + template + inline bool equal(const T& obj1, const T& obj2) { + return obj1.equals(obj2); + } - /** - * This template works for any type with equals - */ - template - bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) - return true; - printf("Not equal:\n"); - expected.print("expected:\n"); - actual.print("actual:\n"); - return false; - } + /** + * This template works for any type with equals + */ + template + bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { + if (actual.equals(expected, tol)) + return true; + printf("Not equal:\n"); + expected.print("expected:\n"); + actual.print("actual:\n"); + return false; + } - /** - * Template to create a binary predicate - */ - template - struct equals : public std::binary_function { - double tol_; - equals(double tol = 1e-9) : tol_(tol) {} - bool operator()(const V& expected, const V& actual) { - return (actual.equals(expected, tol_)); - } - }; + /** + * Template to create a binary predicate + */ + template + struct equals : public std::binary_function { + double tol_; + equals(double tol = 1e-9) : tol_(tol) {} + bool operator()(const V& expected, const V& actual) { + return (actual.equals(expected, tol_)); + } + }; - /** - * Binary predicate on shared pointers - */ - template - struct equals_star : public std::binary_function&, const boost::shared_ptr&, bool> { - double tol_; - equals_star(double tol = 1e-9) : tol_(tol) {} - bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { - if (!actual || !expected) return false; - return (actual->equals(*expected, tol_)); - } - }; + /** + * Binary predicate on shared pointers + */ + template + struct equals_star : public std::binary_function&, const boost::shared_ptr&, bool> { + double tol_; + equals_star(double tol = 1e-9) : tol_(tol) {} + bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { + if (!actual || !expected) return false; + return (actual->equals(*expected, tol_)); + } + }; } // \namespace gtsam diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 00ab79d30..7308b89a7 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -47,36 +47,36 @@ inline bool assert_equal(const Index& expected, const Index& actual, double tol */ template bool assert_equal(const boost::optional& expected, - const boost::optional& actual, double tol = 1e-9) { - if (!expected && actual) { - std::cout << "expected is boost::none, while actual is not" << std::endl; - return false; - } - if (expected && !actual) { - std::cout << "actual is boost::none, while expected is not" << std::endl; - return false; - } - if (!expected && !actual) - return true; - return assert_equal(*expected, *actual, tol); + const boost::optional& actual, double tol = 1e-9) { + if (!expected && actual) { + std::cout << "expected is boost::none, while actual is not" << std::endl; + return false; + } + if (expected && !actual) { + std::cout << "actual is boost::none, while expected is not" << std::endl; + return false; + } + if (!expected && !actual) + return true; + return assert_equal(*expected, *actual, tol); } template bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } template bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } /** @@ -97,15 +97,15 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, } } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; } + std::cout << std::endl; + return false; + } + return true; } /** @@ -114,37 +114,37 @@ bool assert_equal(const std::vector& expected, const std::vector& actual, */ template bool assert_container_equal(const std::map& expected, const std::map& actual, double tol = 1e-9) { - typedef typename std::map Map; + typedef typename std::map Map; bool match = true; if (expected.size() != actual.size()) match = false; typename Map::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (!assert_equal(itExp->first, itAct->first, tol) || - !assert_equal(itExp->second, itAct->second, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (!assert_equal(itExp->first, itAct->first, tol) || + !assert_equal(itExp->second, itAct->second, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { - a.first.print("key"); - a.second.print(" value"); - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { - a.first.print("key"); - a.second.print(" value"); - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + a.first.print("key"); + a.second.print(" value"); + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + a.first.print("key"); + a.second.print(" value"); + } + std::cout << std::endl; + return false; + } + return true; } /** @@ -152,37 +152,37 @@ bool assert_container_equal(const std::map& expected, const std::map bool assert_container_equal(const std::map& expected, const std::map& actual, double tol = 1e-9) { - typedef typename std::map Map; + typedef typename std::map Map; bool match = true; if (expected.size() != actual.size()) match = false; typename Map::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (itExp->first != itAct->first || - !assert_equal(itExp->second, itAct->second, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (itExp->first != itAct->first || + !assert_equal(itExp->second, itAct->second, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { - std::cout << "Key: " << a.first << std::endl; - a.second.print(" value"); - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { - std::cout << "Key: " << a.first << std::endl; - a.second.print(" value"); - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + std::cout << "Key: " << a.first << std::endl; + a.second.print(" value"); + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + std::cout << "Key: " << a.first << std::endl; + a.second.print(" value"); + } + std::cout << std::endl; + return false; + } + return true; } /** @@ -190,38 +190,38 @@ bool assert_container_equal(const std::map& expected, const std::map< */ template bool assert_container_equal(const std::vector >& expected, - const std::vector >& actual, double tol = 1e-9) { - typedef typename std::vector > VectorPair; + const std::vector >& actual, double tol = 1e-9) { + typedef typename std::vector > VectorPair; bool match = true; if (expected.size() != actual.size()) match = false; typename VectorPair::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (!assert_equal(itExp->first, itAct->first, tol) || - !assert_equal(itExp->second, itAct->second, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (!assert_equal(itExp->first, itAct->first, tol) || + !assert_equal(itExp->second, itAct->second, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename VectorPair::value_type& a, expected) { - a.first.print( " first "); - a.second.print(" second"); - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename VectorPair::value_type& a, actual) { - a.first.print( " first "); - a.second.print(" second"); - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename VectorPair::value_type& a, expected) { + a.first.print( " first "); + a.second.print(" second"); + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename VectorPair::value_type& a, actual) { + a.first.print( " first "); + a.second.print(" second"); + } + std::cout << std::endl; + return false; + } + return true; } @@ -234,25 +234,25 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- if (expected.size() != actual.size()) match = false; typename V::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (!assert_equal(*itExp, *itAct, tol)) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (!assert_equal(*itExp, *itAct, tol)) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, expected) { a.print(" "); } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, actual) { a.print(" "); } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, expected) { a.print(" "); } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, actual) { a.print(" "); } + std::cout << std::endl; + return false; + } + return true; } /** @@ -261,36 +261,36 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e- */ template bool assert_container_equality(const std::map& expected, const std::map& actual) { - typedef typename std::map Map; + typedef typename std::map Map; bool match = true; if (expected.size() != actual.size()) match = false; typename Map::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (itExp->first != itAct->first || itExp->second != itAct->second) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (itExp->first != itAct->first || itExp->second != itAct->second) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, expected) { - std::cout << "Key: " << a.first << std::endl; - std::cout << "Value: " << a.second << std::endl; - } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename Map::value_type& a, actual) { - std::cout << "Key: " << a.first << std::endl; - std::cout << "Value: " << a.second << std::endl; - } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, expected) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename Map::value_type& a, actual) { + std::cout << "Key: " << a.first << std::endl; + std::cout << "Value: " << a.second << std::endl; + } + std::cout << std::endl; + return false; + } + return true; } @@ -303,37 +303,37 @@ bool assert_container_equality(const V& expected, const V& actual) { if (expected.size() != actual.size()) match = false; typename V::const_iterator - itExp = expected.begin(), - itAct = actual.begin(); + itExp = expected.begin(), + itAct = actual.begin(); if(match) { - for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { - if (*itExp != *itAct) { - match = false; - break; - } - } + for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { + if (*itExp != *itAct) { + match = false; + break; + } + } } - if(!match) { - std::cout << "expected: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; } - std::cout << "\nactual: " << std::endl; - BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; } - std::cout << std::endl; - return false; - } - return true; + if(!match) { + std::cout << "expected: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; } + std::cout << "\nactual: " << std::endl; + BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; } + std::cout << std::endl; + return false; + } + return true; } /** * Compare strings for unit tests */ inline bool assert_equal(const std::string& expected, const std::string& actual) { - if (expected == actual) - return true; - printf("Not equal:\n"); - std::cout << "expected: [" << expected << "]\n"; - std::cout << "actual: [" << actual << "]" << std::endl; - return false; + if (expected == actual) + return true; + printf("Not equal:\n"); + std::cout << "expected: [" << expected << "]\n"; + std::cout << "actual: [" << actual << "]" << std::endl; + return false; } /** @@ -341,12 +341,12 @@ inline bool assert_equal(const std::string& expected, const std::string& actual) */ template bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) { - if (!actual.equals(expected, tol)) - return true; - printf("Erroneously equal:\n"); - expected.print("expected"); - actual.print("actual"); - return false; + if (!actual.equals(expected, tol)) + return true; + printf("Erroneously equal:\n"); + expected.print("expected"); + actual.print("actual"); + return false; } } // \namespace gtsam diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 8b19a6532..53c374497 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -107,8 +107,8 @@ namespace gtsam { /** Deallocate a raw pointer of this value */ virtual void deallocate_() const = 0; - /** Clone this value (normal clone on the heap, delete with 'delete' operator) */ - virtual boost::shared_ptr clone() const = 0; + /** Clone this value (normal clone on the heap, delete with 'delete' operator) */ + virtual boost::shared_ptr clone() const = 0; /** Compare this Value with another for equality. */ virtual bool equals_(const Value& other, double tol = 1e-9) const = 0; @@ -146,39 +146,39 @@ namespace gtsam { virtual ~Value() {} private: - /** Empty serialization function. - * - * There are two important things that users need to do to serialize derived objects in Values successfully: - * (Those derived objects are stored in Values as pointer to this abstract base class Value) - * - * 1. All DERIVED classes derived from Value must put the following line in their serialization function: - * \code - ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object(*this)); - \endcode - * or, alternatively - * \code - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); - \endcode - * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting - * - * 2. The source module that includes archive class headers to serialize objects of derived classes - * (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either - * BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros: - \code - BOOST_CLASS_EXPORT(DERIVED_CLASS_1) - BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING") - \endcode - * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers - * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export - * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\ - * http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export - * http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export - * The last two links explain why these export lines have to be in the same source module that includes - * any of the archive class headers. - * */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) {} + /** Empty serialization function. + * + * There are two important things that users need to do to serialize derived objects in Values successfully: + * (Those derived objects are stored in Values as pointer to this abstract base class Value) + * + * 1. All DERIVED classes derived from Value must put the following line in their serialization function: + * \code + ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object(*this)); + \endcode + * or, alternatively + * \code + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); + \endcode + * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting + * + * 2. The source module that includes archive class headers to serialize objects of derived classes + * (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either + * BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros: + \code + BOOST_CLASS_EXPORT(DERIVED_CLASS_1) + BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING") + \endcode + * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers + * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export + * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\ + * http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export + * http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export + * The last two links explain why these export lines have to be in the same source module that includes + * any of the archive class headers. + * */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) {} }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index a7e8f2769..82d503837 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -41,42 +41,42 @@ namespace gtsam { /* ************************************************************************* */ void odprintf_(const char *format, ostream& stream, ...) { - char buf[4096], *p = buf; + char buf[4096], *p = buf; - va_list args; - va_start(args, stream); + va_list args; + va_start(args, stream); #ifdef WIN32 - _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #else - vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #endif - va_end(args); + va_end(args); //#ifdef WIN32 -// OutputDebugString(buf); +// OutputDebugString(buf); //#else - stream << buf; + stream << buf; //#endif } /* ************************************************************************* */ void odprintf(const char *format, ...) { - char buf[4096], *p = buf; + char buf[4096], *p = buf; - va_list args; - va_start(args, format); + va_list args; + va_start(args, format); #ifdef WIN32 - _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #else - vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #endif - va_end(args); + va_end(args); //#ifdef WIN32 -// OutputDebugString(buf); +// OutputDebugString(buf); //#else - cout << buf; + cout << buf; //#endif } @@ -89,343 +89,343 @@ Vector Vector_( size_t m, const double* const data) { /* ************************************************************************* */ Vector Vector_(size_t m, ...) { - Vector v(m); - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - v(i) = value; - } - va_end(ap); - return v; + Vector v(m); + va_list ap; + va_start(ap, m); + for( size_t i = 0 ; i < m ; i++) { + double value = va_arg(ap, double); + v(i) = value; + } + va_end(ap); + return v; } /* ************************************************************************* */ Vector Vector_(const std::vector& d) { - Vector v(d.size()); - copy(d.begin(), d.end(), v.data()); - return v; + Vector v(d.size()); + copy(d.begin(), d.end(), v.data()); + return v; } /* ************************************************************************* */ bool zero(const Vector& v) { - bool result = true; - size_t n = v.size(); - for( size_t j = 0 ; j < n ; j++) - result = result && (v(j) == 0.0); - return result; + bool result = true; + size_t n = v.size(); + for( size_t j = 0 ; j < n ; j++) + result = result && (v(j) == 0.0); + return result; } /* ************************************************************************* */ Vector repeat(size_t n, double value) { - return Vector::Constant(n, value); + return Vector::Constant(n, value); } /* ************************************************************************* */ Vector delta(size_t n, size_t i, double value) { - return Vector::Unit(n, i) * value; + return Vector::Unit(n, i) * value; } /* ************************************************************************* */ void print(const Vector& v, const string& s, ostream& stream) { - size_t n = v.size(); - odprintf_("%s [", stream, s.c_str()); - for(size_t i=0; i= vec2[i])) - return false; - return true; + size_t m = vec1.size(); + for(size_t i=0; i= vec2[i])) + return false; + return true; } /* ************************************************************************* */ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { - if (vec1.size()!=vec2.size()) return false; - size_t m = vec1.size(); - for(size_t i=0; i tol) - return false; - } - return true; + if (vec1.size()!=vec2.size()) return false; + size_t m = vec1.size(); + for(size_t i=0; i tol) + return false; + } + return true; } /* ************************************************************************* */ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol) { - if (vec1.size()!=vec2.size()) return false; - size_t m = vec1.size(); - for(size_t i=0; i tol) - return false; - } - return true; + if (vec1.size()!=vec2.size()) return false; + size_t m = vec1.size(); + for(size_t i=0; i tol) + return false; + } + return true; } /* ************************************************************************* */ bool assert_equal(const Vector& expected, const Vector& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; - cout << "not equal:" << endl; - print(expected, "expected"); - print(actual, "actual"); - return false; + if (equal_with_abs_tol(expected,actual,tol)) return true; + cout << "not equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; } /* ************************************************************************* */ bool assert_inequal(const Vector& expected, const Vector& actual, double tol) { - if (!equal_with_abs_tol(expected,actual,tol)) return true; - cout << "Erroneously equal:" << endl; - print(expected, "expected"); - print(actual, "actual"); - return false; + if (!equal_with_abs_tol(expected,actual,tol)) return true; + cout << "Erroneously equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; } /* ************************************************************************* */ bool assert_equal(const SubVector& expected, const SubVector& actual, double tol) { - if (equal_with_abs_tol(expected,actual,tol)) return true; - cout << "not equal:" << endl; - print(expected, "expected"); - print(actual, "actual"); - return false; + if (equal_with_abs_tol(expected,actual,tol)) return true; + cout << "not equal:" << endl; + print(expected, "expected"); + print(actual, "actual"); + return false; } /* ************************************************************************* */ bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual, double tol) { - if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; - cout << "not equal:" << endl; - print(Vector(expected), "expected"); - print(Vector(actual), "actual"); - return false; + if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true; + cout << "not equal:" << endl; + print(Vector(expected), "expected"); + print(Vector(actual), "actual"); + return false; } /* ************************************************************************* */ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { - if (vec1.size()!=vec2.size()) return false; - bool flag = false; double scale = 1.0; - size_t m = vec1.size(); - for(size_t i=0; itol&&fabs(vec2[i])tol)) - return false; - if(vec1[i] == 0 && vec2[i] == 0) continue; - if (!flag) { - scale = vec1[i] / vec2[i]; - flag = true ; - } - else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; - } - return flag; + if (vec1.size()!=vec2.size()) return false; + bool flag = false; double scale = 1.0; + size_t m = vec1.size(); + for(size_t i=0; itol&&fabs(vec2[i])tol)) + return false; + if(vec1[i] == 0 && vec2[i] == 0) continue; + if (!flag) { + scale = vec1[i] / vec2[i]; + flag = true ; + } + else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; + } + return flag; } /* ************************************************************************* */ ConstSubVector sub(const Vector &v, size_t i1, size_t i2) { - return v.segment(i1,i2-i1); + return v.segment(i1,i2-i1); } /* ************************************************************************* */ void subInsert(Vector& fullVector, const Vector& subVector, size_t i) { - fullVector.segment(i, subVector.size()) = subVector; + fullVector.segment(i, subVector.size()) = subVector; } /* ************************************************************************* */ Vector emul(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseProduct(b); + assert (b.size()==a.size()); + return a.cwiseProduct(b); } /* ************************************************************************* */ Vector ediv(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseQuotient(b); + assert (b.size()==a.size()); + return a.cwiseQuotient(b); } /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { - size_t n = a.size(); - assert (b.size()==a.size()); - Vector c(n); - for( size_t i = 0; i < n; i++ ) { - const double &ai = a(i), &bi = b(i); - c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi; - } - return c; + size_t n = a.size(); + assert (b.size()==a.size()); + Vector c(n); + for( size_t i = 0; i < n; i++ ) { + const double &ai = a(i), &bi = b(i); + c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi; + } + return c; } /* ************************************************************************* */ double sum(const Vector &a) { - return a.sum(); + return a.sum(); } /* ************************************************************************* */ double norm_2(const Vector& v) { - return v.norm(); + return v.norm(); } /* ************************************************************************* */ Vector reciprocal(const Vector &a) { - size_t n = a.size(); - Vector b(n); - for( size_t i = 0; i < n; i++ ) - b(i) = 1.0/a(i); - return b; + size_t n = a.size(); + Vector b(n); + for( size_t i = 0; i < n; i++ ) + b(i) = 1.0/a(i); + return b; } /* ************************************************************************* */ Vector esqrt(const Vector& v) { - return v.cwiseSqrt(); + return v.cwiseSqrt(); } /* ************************************************************************* */ Vector abs(const Vector& v) { - return v.cwiseAbs(); + return v.cwiseAbs(); } /* ************************************************************************* */ double max(const Vector &a) { - return a.maxCoeff(); + return a.maxCoeff(); } /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { - const double x0 = v(0); - const double x02 = x0*x0; + const double x0 = v(0); + const double x02 = x0*x0; - // old code - GSL verison was actually a bit slower - const double sigma = v.squaredNorm() - x02; + // old code - GSL verison was actually a bit slower + const double sigma = v.squaredNorm() - x02; - v(0) = 1.0; + v(0) = 1.0; - if( sigma == 0.0 ) - return 0.0; - else { - double mu = sqrt(x02 + sigma); - if( x0 <= 0.0 ) - v(0) = x0 - mu; - else - v(0) = -sigma / (x0 + mu); + if( sigma == 0.0 ) + return 0.0; + else { + double mu = sqrt(x02 + sigma); + if( x0 <= 0.0 ) + v(0) = x0 - mu; + else + v(0) = -sigma / (x0 + mu); - const double v02 = v(0)*v(0); - v = v / v(0); - return 2.0 * v02 / (sigma + v02); - } + const double v02 = v(0)*v(0); + v = v / v(0); + return 2.0 * v02 / (sigma + v02); + } } /* ************************************************************************* */ pair house(const Vector &x) { - Vector v(x); - double beta = houseInPlace(v); - return make_pair(beta, v); + Vector v(x); + double beta = houseInPlace(v); + return make_pair(beta, v); } /* ************************************************************************* */ // Fast version *no error checking* ! // Pass in initialized vector of size m or will crash ! double weightedPseudoinverse(const Vector& a, const Vector& weights, - Vector& pseudo) { + Vector& pseudo) { - size_t m = weights.size(); - static const double inf = std::numeric_limits::infinity(); + size_t m = weights.size(); + static const double inf = std::numeric_limits::infinity(); - // Check once for zero entries of a. TODO: really needed ? - vector isZero; - for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); + // Check once for zero entries of a. TODO: really needed ? + vector isZero; + for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); - // If there is a valid (a!=0) constraint (sigma==0) return the first one - for (size_t i = 0; i < m; ++i) - if (weights[i] == inf && !isZero[i]) { - pseudo = delta(m, i, 1 / a[i]); - return inf; - } + // If there is a valid (a!=0) constraint (sigma==0) return the first one + for (size_t i = 0; i < m; ++i) + if (weights[i] == inf && !isZero[i]) { + pseudo = delta(m, i, 1 / a[i]); + return inf; + } - // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) - // For diagonal Sigma, inv(Sigma) = diag(precisions) - double precision = 0; - for (size_t i = 0; i < m; i++) { - double ai = a[i]; - if (!isZero[i]) // also catches remaining sigma==0 rows - precision += weights[i] * (ai * ai); - } + // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) + // For diagonal Sigma, inv(Sigma) = diag(precisions) + double precision = 0; + for (size_t i = 0; i < m; i++) { + double ai = a[i]; + if (!isZero[i]) // also catches remaining sigma==0 rows + precision += weights[i] * (ai * ai); + } - // precision = a'inv(Sigma)a - if (precision < 1e-9) - for (size_t i = 0; i < m; i++) - pseudo[i] = 0; - else { - // emul(precisions,a)/precision - double variance = 1.0 / precision; - for (size_t i = 0; i < m; i++) - pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i]; - } - return precision; // sum of weights + // precision = a'inv(Sigma)a + if (precision < 1e-9) + for (size_t i = 0; i < m; i++) + pseudo[i] = 0; + else { + // emul(precisions,a)/precision + double variance = 1.0 / precision; + for (size_t i = 0; i < m; i++) + pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i]; + } + return precision; // sum of weights } /* ************************************************************************* */ // Slow version with error checking pair weightedPseudoinverse(const Vector& a, const Vector& weights) { - int m = weights.size(); - if (a.size() != m) - throw invalid_argument("a and weights have different sizes!"); - Vector pseudo(m); - double precision = weightedPseudoinverse(a, weights, pseudo); - return make_pair(pseudo, precision); + int m = weights.size(); + if (a.size() != m) + throw invalid_argument("a and weights have different sizes!"); + Vector pseudo(m); + double precision = weightedPseudoinverse(a, weights, pseudo); + return make_pair(pseudo, precision); } /* ************************************************************************* */ Vector concatVectors(const std::list& vs) { - size_t dim = 0; - BOOST_FOREACH(Vector v, vs) - dim += v.size(); + size_t dim = 0; + BOOST_FOREACH(Vector v, vs) + dim += v.size(); - Vector A(dim); - size_t index = 0; - BOOST_FOREACH(Vector v, vs) { - for(int d = 0; d < v.size(); d++) - A(d+index) = v(d); - index += v.size(); - } + Vector A(dim); + size_t index = 0; + BOOST_FOREACH(Vector v, vs) { + for(int d = 0; d < v.size(); d++) + A(d+index) = v(d); + index += v.size(); + } - return A; + return A; } /* ************************************************************************* */ Vector concatVectors(size_t nrVectors, ...) { - va_list ap; - list vs; - va_start(ap, nrVectors); - for( size_t i = 0 ; i < nrVectors ; i++) { - Vector* V = va_arg(ap, Vector*); - vs.push_back(*V); - } - va_end(ap); - return concatVectors(vs); + va_list ap; + list vs; + va_start(ap, nrVectors); + for( size_t i = 0 ; i < nrVectors ; i++) { + Vector* V = va_arg(ap, Vector*); + vs.push_back(*V); + } + va_end(ap); + return concatVectors(vs); } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 0d2d11f98..a52b90f08 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -98,7 +98,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);} * @param n size */ inline Vector ones(size_t n) { return Vector::Ones(n); } - + /** * check if all zero */ @@ -277,15 +277,15 @@ double max(const Vector &a); */ template inline double dot(const V1 &a, const V2& b) { - assert (b.size()==a.size()); - return a.dot(b); + assert (b.size()==a.size()); + return a.dot(b); } /** compatibility version for ublas' inner_prod() */ template inline double inner_prod(const V1 &a, const V2& b) { - assert (b.size()==a.size()); - return a.dot(b); + assert (b.size()==a.size()); + return a.dot(b); } /** @@ -300,12 +300,12 @@ inline void scal(double alpha, Vector& x) { x *= alpha; } */ template inline void axpy(double alpha, const V1& x, V2& y) { - assert (y.size()==x.size()); - y += alpha * x; + assert (y.size()==x.size()); + y += alpha * x; } inline void axpy(double alpha, const Vector& x, SubVector y) { - assert (y.size()==x.size()); - y += alpha * x; + assert (y.size()==x.size()); + y += alpha * x; } /** @@ -360,17 +360,17 @@ namespace serialization { template void save(Archive & ar, const gtsam::Vector & v, unsigned int version) { - const size_t n = v.size(); - std::vector raw_data(n); - copy(v.data(), v.data()+n, raw_data.begin()); - ar << make_nvp("data", raw_data); + const size_t n = v.size(); + std::vector raw_data(n); + copy(v.data(), v.data()+n, raw_data.begin()); + ar << make_nvp("data", raw_data); } template void load(Archive & ar, gtsam::Vector & v, unsigned int version) { - std::vector raw_data; - ar >> make_nvp("data", raw_data); - v = gtsam::Vector_(raw_data); + std::vector raw_data; + ar >> make_nvp("data", raw_data); + v = gtsam::Vector_(raw_data); } } // namespace serialization diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h index c0bc95a50..a23f71e43 100644 --- a/gtsam/base/blockMatrices.h +++ b/gtsam/base/blockMatrices.h @@ -307,11 +307,11 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(rowStart_); - ar & BOOST_SERIALIZATION_NVP(rowEnd_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(rowStart_); + ar & BOOST_SERIALIZATION_NVP(rowEnd_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); } }; @@ -377,9 +377,9 @@ public: blockStart_ = 0; fillOffsets(firstBlockDim, lastBlockDim); if (preserve) - matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back()); + matrix_.conservativeResize(variableColOffsets_.back(), variableColOffsets_.back()); else - matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); } /** Row size @@ -415,9 +415,9 @@ public: assert(i_actualEndBlock < variableColOffsets_.size()); assert(j_actualEndBlock < variableColOffsets_.size()); return matrix_.block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); + variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], + variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); } constBlock range(size_t i_startBlock, size_t i_endBlock, size_t j_startBlock, size_t j_endBlock) const { @@ -431,9 +431,9 @@ public: assert(i_actualEndBlock < variableColOffsets_.size()); assert(j_actualEndBlock < variableColOffsets_.size()); return ((const FullMatrix&)matrix_).block( - variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], - variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); + variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock], + variableColOffsets_[j_actualEndBlock]-variableColOffsets_[j_actualStartBlock]); } Block full() { @@ -448,19 +448,19 @@ public: const FullMatrix& fullMatrix() const { return matrix_; } Column column(size_t i_block, size_t j_block, size_t columnOffset) { - assertInvariants(); - size_t i_actualBlock = i_block + blockStart_; - size_t j_actualBlock = j_block + blockStart_; - checkBlock(i_actualBlock); - checkBlock(j_actualBlock); - assert(i_actualBlock < variableColOffsets_.size()); - assert(j_actualBlock < variableColOffsets_.size()); - assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); + assertInvariants(); + size_t i_actualBlock = i_block + blockStart_; + size_t j_actualBlock = j_block + blockStart_; + checkBlock(i_actualBlock); + checkBlock(j_actualBlock); + assert(i_actualBlock < variableColOffsets_.size()); + assert(j_actualBlock < variableColOffsets_.size()); + assert(variableColOffsets_[j_actualBlock] + columnOffset < variableColOffsets_[j_actualBlock+1]); - return matrix_.col( - variableColOffsets_[j_actualBlock] + columnOffset).segment( - variableColOffsets_[i_actualBlock], - variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); + return matrix_.col( + variableColOffsets_[j_actualBlock] + columnOffset).segment( + variableColOffsets_[i_actualBlock], + variableColOffsets_[i_actualBlock+1]-variableColOffsets_[i_actualBlock]); } constColumn column(size_t i_block, size_t j_block, size_t columnOffset) const { @@ -496,9 +496,9 @@ public: assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); return matrix_.col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); + variableColOffsets_[j_actualStartBlock] + columnOffset).segment( + variableColOffsets_[i_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); } constColumn rangeColumn(size_t i_startBlock, size_t i_endBlock, size_t j_block, size_t columnOffset) const { @@ -513,9 +513,9 @@ public: assert(variableColOffsets_[j_actualStartBlock] + columnOffset < variableColOffsets_[j_actualStartBlock+1]); return ((const FullMatrix&)matrix_).col( - variableColOffsets_[j_actualStartBlock] + columnOffset).segment( - variableColOffsets_[i_actualStartBlock], - variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); + variableColOffsets_[j_actualStartBlock] + columnOffset).segment( + variableColOffsets_[i_actualStartBlock], + variableColOffsets_[i_actualEndBlock]-variableColOffsets_[i_actualStartBlock]); } size_t offset(size_t block) const { @@ -615,9 +615,9 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); - ar & BOOST_SERIALIZATION_NVP(blockStart_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); } }; diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index e941082ae..f73b6de78 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -33,22 +33,22 @@ namespace gtsam { static const double negativePivotThreshold = -1e-1; static const double zeroPivotThreshold = 1e-6; static const double underconstrainedPrior = 1e-5; - static const int underconstrainedExponentDifference = 12; + static const int underconstrainedExponentDifference = 12; /* ************************************************************************* */ static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) { - const bool debug = ISDEBUG("choleskyCareful"); + const bool debug = ISDEBUG("choleskyCareful"); // Get pivot value double alpha = ATA(k,k); // Correct negative pivots from round-off error if(alpha < negativePivotThreshold) { - if(debug) { - cout << "pivot = " << alpha << endl; - print(ATA, "Partially-factorized matrix: "); - } + if(debug) { + cout << "pivot = " << alpha << endl; + print(ATA, "Partially-factorized matrix: "); + } return -1; } else if(alpha < 0.0) alpha = 0.0; @@ -63,23 +63,23 @@ static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) { if(k < (order-1)) { // Update A(k,k+1:end) <- A(k,k+1:end) / beta - typedef Matrix::RowXpr::SegmentReturnType BlockRow; - BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); - V *= betainv; + typedef Matrix::RowXpr::SegmentReturnType BlockRow; + BlockRow V = ATA.row(k).segment(k+1, order-(k+1)); + V *= betainv; // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha - ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; -// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() -// .rankUpdate(V.adjoint(), -1); + ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; +// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() +// .rankUpdate(V.adjoint(), -1); } - return 1; + return 1; } else { // For zero pivots, add the underconstrained variable prior ATA(k,k) = underconstrainedPrior; for(size_t j=k+1; j choleskyCareful(Matrix& ATA, int order) { // The index of the row after the last non-zero row of the square-root factor size_t maxrank = 0; - bool success = true; + bool success = true; // Factor row-by-row for(size_t k = 0; k < size_t(order); ++k) { int stepResult = choleskyStep(ATA, k, size_t(order)); - if(stepResult == 1) { + if(stepResult == 1) { if(debug) cout << "choleskyCareful: Factored through " << k << endl; if(debug) print(ATA, "ATA: "); maxrank = k+1; } else if(stepResult == -1) { success = false; - break; + break; } /* else if(stepResult == 0) Found zero pivot */ } @@ -132,7 +132,7 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { // Compute Cholesky factorization of A, overwrites A. tic(1, "lld"); - Eigen::LLT llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt(); + Eigen::LLT llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt(); ABC.block(0,0,nFrontal,nFrontal).triangularView() = llt.matrixU(); toc(1, "lld"); @@ -156,26 +156,26 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal) { if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; toc(3, "compute L"); - // Check last diagonal element - Eigen does not check it - bool ok; - if(llt.info() == Eigen::Success) { - if(nFrontal >= 2) { - int exp2, exp1; - (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); - (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); - ok = (exp2 - exp1 < underconstrainedExponentDifference); - } else if(nFrontal == 1) { - int exp1; - (void)frexp(ABC(0,0), &exp1); - ok = (exp1 > -underconstrainedExponentDifference); - } else { - ok = true; - } - } else { - ok = false; - } + // Check last diagonal element - Eigen does not check it + bool ok; + if(llt.info() == Eigen::Success) { + if(nFrontal >= 2) { + int exp2, exp1; + (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); + (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); + ok = (exp2 - exp1 < underconstrainedExponentDifference); + } else if(nFrontal == 1) { + int exp1; + (void)frexp(ABC(0,0), &exp1); + ok = (exp1 > -underconstrainedExponentDifference); + } else { + ok = true; + } + } else { + ok = false; + } - return ok; + return ok; } } diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index b1992f741..302ac32ea 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -29,23 +29,23 @@ namespace gtsam { namespace testing { - /** binary functions */ - template - T between(const T& t1, const T& t2) { return t1.between(t2); } + /** binary functions */ + template + T between(const T& t1, const T& t2) { return t1.between(t2); } - template - T compose(const T& t1, const T& t2) { return t1.compose(t2); } + template + T compose(const T& t1, const T& t2) { return t1.compose(t2); } - /** unary functions */ - template - T inverse(const T& t) { return t.inverse(); } + /** unary functions */ + template + T inverse(const T& t) { return t.inverse(); } - /** rotation functions */ - template - P rotate(const T& r, const P& pt) { return r.rotate(pt); } + /** rotation functions */ + template + P rotate(const T& r, const P& pt) { return r.rotate(pt); } - template - P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } + template + P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } } // \namespace testing } // \namespace gtsam diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 86abf6067..6b6a377de 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -32,431 +32,431 @@ namespace gtsam { - /* - * Note that all of these functions have two versions, a boost.function version and a - * standard C++ function pointer version. This allows reformulating the arguments of - * a function to fit the correct structure, which is useful for situations like - * member functions and functions with arguments not involved in the derivative: - * - * Usage of the boost bind version to rearrange arguments: - * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) - * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided - * - * For member functions, such as below, with an instantiated copy instanceOfSomeClass - * Foo SomeClass::bar(const Obj& a) - * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) - * - * For additional details, see the documentation: - * http://www.boost.org/doc/libs/release/libs/bind/bind.html - */ + /* + * Note that all of these functions have two versions, a boost.function version and a + * standard C++ function pointer version. This allows reformulating the arguments of + * a function to fit the correct structure, which is useful for situations like + * member functions and functions with arguments not involved in the derivative: + * + * Usage of the boost bind version to rearrange arguments: + * for a function with one relevant param and an optional derivative: + * Foo bar(const Obj& a, boost::optional H1) + * Use boost.bind to restructure: + * boost::bind(bar, _1, boost::none) + * This syntax will fix the optional argument to boost::none, while using the first argument provided + * + * For member functions, such as below, with an instantiated copy instanceOfSomeClass + * Foo SomeClass::bar(const Obj& a) + * Use boost bind as follows to create a function pointer that uses the member function: + * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * + * For additional details, see the documentation: + * http://www.boost.org/doc/libs/release/libs/bind/bind.html + */ - /** global functions for converting to a LieVector for use with numericalDerivative */ + /** global functions for converting to a LieVector for use with numericalDerivative */ inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } + inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } - /** - * Numerically compute gradient of scalar function - * Class X is the input argument - * The class X needs to have dim, expmap, logmap - */ - template - Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { - double factor = 1.0/(2.0*delta); - const size_t n = x.dim(); - Vector d = zero(n), g = zero(n); - for (size_t j=0;j + Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { + double factor = 1.0/(2.0*delta); + const size_t n = x.dim(); + Vector d = zero(n), g = zero(n); + for (size_t j=0;j - Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalGradient(boost::bind(h, _1), x, delta); - } + template + Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { + return numericalGradient(boost::bind(h, _1), x, delta); + } - /** - * Compute numerical derivative in argument 1 of unary function - * @param h unary function yielding m-vector - * @param x n-dimensional value at which to evaluate h - * @param delta increment for numerical derivative - * Class Y is the output argument - * Class X is the input argument - * @return m*n Jacobian computed via central differencing - * Both classes X,Y need dim, expmap, logmap - */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - Y hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + Y hx = h(x); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j - Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); - } + /** use a raw C++ function pointer */ + template + Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(h, _1), x, delta); + } - /** remapping for double valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } + /** remapping for double valued functions */ + template + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); + } - template - Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } + template + Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); + } - /** remapping for vector valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } + /** remapping for vector valued functions */ + template + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); + } - template - Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } + template + Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); + } - /** - * Compute numerical derivative in argument 1 of binary function - * @param h binary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + Y hx = h(x1,x2); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x1.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j - inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); - } + /** use a raw C++ function pointer */ + template + inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - Matrix numericalDerivative21(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + Matrix numericalDerivative21(double (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** - * Compute numerical derivative in argument 2 of binary function - * @param h binary function yielding m-vector - * @param x1 first argument value - * @param x2 n-dimensional second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative22 - (boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j + Matrix numericalDerivative22 + (boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + Y hx = h(x1,x2); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x2.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j - inline Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); - } + /** use a raw C++ function pointer */ + template + inline Matrix numericalDerivative22 + (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - template - inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } + template + inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } - /** - * Compute numerical derivative in argument 1 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative31 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative31 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } + /** + * Compute numerical derivative in argument 1 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ + template + Matrix numericalDerivative31 + (boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + { + Y hx = h(x1,x2,x3); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x1.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j + inline Matrix numericalDerivative31 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative31(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative31(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** - * Compute numerical derivative in argument 2 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative32 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative32 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } + /** + * Compute numerical derivative in argument 2 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ + template + Matrix numericalDerivative32 + (boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + { + Y hx = h(x1,x2,x3); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x2.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j + inline Matrix numericalDerivative32 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative32(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative32(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** - * Compute numerical derivative in argument 3 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative33 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x3.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative33 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } + /** + * Compute numerical derivative in argument 3 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ + template + Matrix numericalDerivative33 + (boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + { + Y hx = h(x1,x2,x3); + double factor = 1.0/(2.0*delta); + const size_t m = hx.dim(), n = x3.dim(); + Vector d = zero(n); + Matrix H = zeros(m,n); + for (size_t j=0;j + inline Matrix numericalDerivative33 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative33(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative33(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } - template - inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } + template + inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative33( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar @@ -466,17 +466,17 @@ namespace gtsam { * @param delta The numerical derivative step size * @return n*n Hessian matrix computed via central differencing */ - template - inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::function(boost::bind( - static_cast,const X&, double)>(&numericalGradient), - f, _1, delta)), x, delta); - } + template + inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::function(boost::bind( + static_cast,const X&, double)>(&numericalGradient), + f, _1, delta)), x, delta); + } - template - inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { - return numericalHessian(boost::function(f), x, delta); - } + template + inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { + return numericalHessian(boost::function(f), x, delta); + } /** Helper class that computes the derivative of f w.r.t. x1, centered about diff --git a/gtsam/base/tests/testBlockMatrices.cpp b/gtsam/base/tests/testBlockMatrices.cpp index 06b3f3002..3950d561a 100644 --- a/gtsam/base/tests/testBlockMatrices.cpp +++ b/gtsam/base/tests/testBlockMatrices.cpp @@ -23,15 +23,15 @@ using namespace gtsam; /* ************************************************************************* */ TEST(testBlockMatrices, jacobian_factor1) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; + typedef Matrix AbMatrix; + typedef VerticalBlockView BlockAb; - AbMatrix matrix; // actual matrix - empty to start with + AbMatrix matrix; // actual matrix - empty to start with // from JacobianFactor::Constructor - one variable Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); + 1., 2., 3., + 4., 5., 6.); Vector b = Vector_(2, 7., 8.); size_t dims[] = { A1.cols(), 1}; @@ -50,26 +50,26 @@ TEST(testBlockMatrices, jacobian_factor1) { // examine matrix contents EXPECT_LONGS_EQUAL(2, Ab.nBlocks()); Matrix expFull = Matrix_(2, 4, - 1., 2., 3., 7., - 4., 5., 6., 8.); + 1., 2., 3., 7., + 4., 5., 6., 8.); Matrix actFull = Ab.full(); EXPECT(assert_equal(expFull, actFull)); } /* ************************************************************************* */ TEST(testBlockMatrices, jacobian_factor2) { - typedef Matrix AbMatrix; - typedef VerticalBlockView BlockAb; + typedef Matrix AbMatrix; + typedef VerticalBlockView BlockAb; - AbMatrix matrix; // actual matrix - empty to start with + AbMatrix matrix; // actual matrix - empty to start with // from JacobianFactor::Constructor - two variables Matrix A1 = Matrix_(2,3, - 1., 2., 3., - 4., 5., 6.); + 1., 2., 3., + 4., 5., 6.); Matrix A2 = Matrix_(2,1, - 10., - 11.); + 10., + 11.); Vector b = Vector_(2, 7., 8.); size_t dims[] = { A1.cols(), A2.cols(), 1}; @@ -91,8 +91,8 @@ TEST(testBlockMatrices, jacobian_factor2) { // examine matrix contents EXPECT_LONGS_EQUAL(3, Ab.nBlocks()); Matrix expFull = Matrix_(2, 5, - 1., 2., 3., 10., 7., - 4., 5., 6., 11., 8.); + 1., 2., 3., 10., 7., + 4., 5., 6., 11., 8.); Matrix actFull = Ab.full(); EXPECT(assert_equal(expFull, actFull)); } @@ -102,29 +102,29 @@ TEST(testBlockMatrices, hessian_factor1) { typedef Matrix InfoMatrix; typedef SymmetricBlockView BlockInfo; - Matrix expected_full = Matrix_(3, 3, - 3.0, 5.0, -8.0, - 0.0, 6.0, -9.0, - 0.0, 0.0, 10.0); + Matrix expected_full = Matrix_(3, 3, + 3.0, 5.0, -8.0, + 0.0, 6.0, -9.0, + 0.0, 0.0, 10.0); Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); Vector g = Vector_(2, -8.0, -9.0); double f = 10.0; - size_t dims[] = { G.rows(), 1 }; - InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1); - BlockInfo infoView(fullMatrix, dims, dims+2); - infoView(0,0) = G; - infoView.column(0,1,0) = g; - infoView(1,1)(0,0) = f; + size_t dims[] = { G.rows(), 1 }; + InfoMatrix fullMatrix = zeros(G.rows() + 1, G.rows() + 1); + BlockInfo infoView(fullMatrix, dims, dims+2); + infoView(0,0) = G; + infoView.column(0,1,0) = g; + infoView(1,1)(0,0) = f; - EXPECT_LONGS_EQUAL(0, infoView.blockStart()); - EXPECT_LONGS_EQUAL(2, infoView.nBlocks()); - EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix)); - EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1))) - EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10); + EXPECT_LONGS_EQUAL(0, infoView.blockStart()); + EXPECT_LONGS_EQUAL(2, infoView.nBlocks()); + EXPECT(assert_equal(InfoMatrix(expected_full), fullMatrix)); + EXPECT(assert_equal(InfoMatrix(G), infoView.range(0, 1, 0, 1))) + EXPECT_DOUBLES_EQUAL(f, infoView(1, 1)(0,0), 1e-10); - EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0)))); + EXPECT(assert_equal(g, Vector(infoView.rangeColumn(0, 1, 1, 0)))); EXPECT(assert_equal(g, Vector(((const BlockInfo)infoView).rangeColumn(0, 1, 1, 0)))); } diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 8e8da7a70..93e8ce2a2 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -86,42 +86,42 @@ TEST(cholesky, BadScalingSVD) { /* ************************************************************************* */ TEST(cholesky, underconstrained) { - Matrix L(6,6); L << - 1, 0, 0, 0, 0, 0, - 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, - 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, - 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, - 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, - 0.1197220685104, 1, 0, 0, 0, 0; - Matrix D1(6,6); D1 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, 1.34342584865901, 0, - 0, 0, 0, 0, 0, 1e-12; - Matrix D2(6,6); D2 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0; - Matrix D3(6,6); D3 << - 0.814723686393179, 0, 0, 0, 0, 0, - 0, 0.811780089277421, 0, 0, 0, 0, - 0, 0, 1.82596950680844, 0, 0, 0, - 0, 0, 0, 0.240287537694585, 0, 0, - 0, 0, 0, 0, -0.5, 0, - 0, 0, 0, 0, 0, -0.6; + Matrix L(6,6); L << + 1, 0, 0, 0, 0, 0, + 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, + 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, + 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, + 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, + 0.1197220685104, 1, 0, 0, 0, 0; + Matrix D1(6,6); D1 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 1.34342584865901, 0, + 0, 0, 0, 0, 0, 1e-12; + Matrix D2(6,6); D2 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0; + Matrix D3(6,6); D3 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, -0.5, 0, + 0, 0, 0, 0, 0, -0.6; - Matrix A1 = L * D1 * L.transpose(); - Matrix A2 = L * D2 * L.transpose(); - Matrix A3 = L * D3 * L.transpose(); + Matrix A1 = L * D1 * L.transpose(); + Matrix A2 = L * D2 * L.transpose(); + Matrix A3 = L * D3 * L.transpose(); - LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6))); - LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6))); - LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index c0b72f1a0..cad2f6ded 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -31,139 +31,139 @@ using namespace gtsam; /* ************************************************************************* */ TEST(DSFVectorVector, findSet) { - DSFVector dsf(3); - CHECK(dsf.findSet(0) != dsf.findSet(2)); + DSFVector dsf(3); + CHECK(dsf.findSet(0) != dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVectorVector, makeUnionInPlace) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,2); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,2); + CHECK(dsf.findSet(0) == dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVectorVector, makeUnionInPlace2) { - boost::shared_ptr v = boost::make_shared(5); - std::vector keys; keys += 1, 3; - DSFVector dsf(v, keys); - dsf.makeUnionInPlace(1,3); - CHECK(dsf.findSet(1) == dsf.findSet(3)); + boost::shared_ptr v = boost::make_shared(5); + std::vector keys; keys += 1, 3; + DSFVector dsf(v, keys); + dsf.makeUnionInPlace(1,3); + CHECK(dsf.findSet(1) == dsf.findSet(3)); } /* ************************************************************************* */ TEST(DSFVector, makeUnion2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(2,0); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(2,0); + CHECK(dsf.findSet(0) == dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVector, makeUnion3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - CHECK(dsf.findSet(0) == dsf.findSet(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + CHECK(dsf.findSet(0) == dsf.findSet(2)); } /* ************************************************************************* */ TEST(DSFVector, sets) { - DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); - map > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); + DSFVector dsf(2); + dsf.makeUnionInPlace(0,1); + map > sets = dsf.sets(); + LONGS_EQUAL(1, sets.size()); - set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); + set expected; expected += 0, 1; + CHECK(expected == sets[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays) { - DSFVector dsf(2); - dsf.makeUnionInPlace(0,1); - map > arrays = dsf.arrays(); - LONGS_EQUAL(1, arrays.size()); + DSFVector dsf(2); + dsf.makeUnionInPlace(0,1); + map > arrays = dsf.arrays(); + LONGS_EQUAL(1, arrays.size()); - vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); + vector expected; expected += 0, 1; + CHECK(expected == arrays[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, sets2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - map > sets = dsf.sets(); - LONGS_EQUAL(1, sets.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + map > sets = dsf.sets(); + LONGS_EQUAL(1, sets.size()); - set expected; expected += 0, 1, 2; - CHECK(expected == sets[dsf.findSet(0)]); + set expected; expected += 0, 1, 2; + CHECK(expected == sets[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - map > arrays = dsf.arrays(); - LONGS_EQUAL(1, arrays.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + map > arrays = dsf.arrays(); + LONGS_EQUAL(1, arrays.size()); - vector expected; expected += 0, 1, 2; - CHECK(expected == arrays[dsf.findSet(0)]); + vector expected; expected += 0, 1, 2; + CHECK(expected == arrays[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, sets3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - map > sets = dsf.sets(); - LONGS_EQUAL(2, sets.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + map > sets = dsf.sets(); + LONGS_EQUAL(2, sets.size()); - set expected; expected += 0, 1; - CHECK(expected == sets[dsf.findSet(0)]); + set expected; expected += 0, 1; + CHECK(expected == sets[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, arrays3) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - map > arrays = dsf.arrays(); - LONGS_EQUAL(2, arrays.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + map > arrays = dsf.arrays(); + LONGS_EQUAL(2, arrays.size()); - vector expected; expected += 0, 1; - CHECK(expected == arrays[dsf.findSet(0)]); + vector expected; expected += 0, 1; + CHECK(expected == arrays[dsf.findSet(0)]); } /* ************************************************************************* */ TEST(DSFVector, set) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - set set = dsf.set(0); - LONGS_EQUAL(2, set.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + set set = dsf.set(0); + LONGS_EQUAL(2, set.size()); - std::set expected; expected += 0, 1; - CHECK(expected == set); + std::set expected; expected += 0, 1; + CHECK(expected == set); } /* ************************************************************************* */ TEST(DSFVector, set2) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - dsf.makeUnionInPlace(1,2); - set set = dsf.set(0); - LONGS_EQUAL(3, set.size()); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + dsf.makeUnionInPlace(1,2); + set set = dsf.set(0); + LONGS_EQUAL(3, set.size()); - std::set expected; expected += 0, 1, 2; - CHECK(expected == set); + std::set expected; expected += 0, 1, 2; + CHECK(expected == set); } /* ************************************************************************* */ TEST(DSFVector, isSingleton) { - DSFVector dsf(3); - dsf.makeUnionInPlace(0,1); - CHECK(!dsf.isSingleton(0)); - CHECK(!dsf.isSingleton(1)); - CHECK( dsf.isSingleton(2)); + DSFVector dsf(3); + dsf.makeUnionInPlace(0,1); + CHECK(!dsf.isSingleton(0)); + CHECK(!dsf.isSingleton(1)); + CHECK( dsf.isSingleton(2)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index b3e065921..464624bd4 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -21,12 +21,12 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; - init_vector += 2, 3, 4, 5; + FastVector init_vector; + init_vector += 2, 3, 4, 5; - FastSet actSet(init_vector); - FastSet expSet; expSet += 2, 3, 4, 5; - EXPECT(actSet == expSet); + FastSet actSet(init_vector); + FastSet expSet; expSet += 2, 3, 4, 5; + EXPECT(actSet == expSet); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index aad28563c..5ef867e85 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -27,24 +27,24 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix) /* ************************************************************************* */ TEST( LieMatrix, construction ) { - Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); - LieMatrix lie1(m), lie2(m); + Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); + LieMatrix lie1(m), lie2(m); - EXPECT(lie1.dim() == 4); - EXPECT(assert_equal(m, lie1.matrix())); - EXPECT(assert_equal(lie1, lie2)); + EXPECT(lie1.dim() == 4); + EXPECT(assert_equal(m, lie1.matrix())); + EXPECT(assert_equal(lie1, lie2)); } /* ************************************************************************* */ TEST( LieMatrix, other_constructors ) { - Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); - LieMatrix exp(init); - LieMatrix a(2,2,10.0,20.0,30.0,40.0); - double data[] = {10,30,20,40}; - LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, a)); - EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); + Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); + LieMatrix exp(init); + LieMatrix a(2,2,10.0,20.0,30.0,40.0); + double data[] = {10,30,20,40}; + LieMatrix b(2,2,data); + EXPECT(assert_equal(exp, a)); + EXPECT(assert_equal(exp, b)); + EXPECT(assert_equal(b, a)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 229737b8d..11157a701 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -29,20 +29,20 @@ const double tol=1e-9; /* ************************************************************************* */ TEST( testLieScalar, construction ) { - double d = 2.; - LieScalar lie1(d), lie2(d); + double d = 2.; + LieScalar lie1(d), lie2(d); - EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); - EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(lie1.dim() == 1); - EXPECT(assert_equal(lie1, lie2)); + EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); + EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); + EXPECT(lie1.dim() == 1); + EXPECT(assert_equal(lie1, lie2)); } /* ************************************************************************* */ TEST( testLieScalar, localCoordinates ) { - LieScalar lie1(1.), lie2(3.); + LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); + EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index c4c88dc9b..74be6628f 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -27,26 +27,26 @@ GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) { - Vector v = Vector_(3, 1.0, 2.0, 3.0); - LieVector lie1(v), lie2(v); + Vector v = Vector_(3, 1.0, 2.0, 3.0); + LieVector lie1(v), lie2(v); - EXPECT(lie1.dim() == 3); - EXPECT(assert_equal(v, lie1.vector())); - EXPECT(assert_equal(lie1, lie2)); + EXPECT(lie1.dim() == 3); + EXPECT(assert_equal(v, lie1.vector())); + EXPECT(assert_equal(lie1, lie2)); } /* ************************************************************************* */ TEST( testLieVector, other_constructors ) { - Vector init = Vector_(2, 10.0, 20.0); - LieVector exp(init); - LieVector a(2,10.0,20.0); - double data[] = {10,20}; - LieVector b(2,data); - LieVector c(2.3), c_exp(LieVector(1, 2.3)); - EXPECT(assert_equal(exp, a)); - EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); - EXPECT(assert_equal(c_exp, c)); + Vector init = Vector_(2, 10.0, 20.0); + LieVector exp(init); + LieVector a(2,10.0,20.0); + double data[] = {10,20}; + LieVector b(2,data); + LieVector c(2.3), c_exp(LieVector(1, 2.3)); + EXPECT(assert_equal(exp, a)); + EXPECT(assert_equal(exp, b)); + EXPECT(assert_equal(b, a)); + EXPECT(assert_equal(c_exp, c)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 56be42c02..4f57a6b6b 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -32,234 +32,234 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( matrix, constructor_data ) { - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); + double data[] = { -5, 3, 0, -5 }; + Matrix A = Matrix_(2, 2, data); - Matrix B(2, 2); - B(0, 0) = -5; - B(0, 1) = 3; - B(1, 0) = 0; - B(1, 1) = -5; + Matrix B(2, 2); + B(0, 0) = -5; + B(0, 1) = 3; + B(1, 0) = 0; + B(1, 1) = -5; - EQUALITY(A,B); + EQUALITY(A,B); } /* ************************************************************************* */ TEST( matrix, constructor_vector ) { - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); - Vector v(4); - copy(data, data + 4, v.data()); - Matrix B = Matrix_(2, 2, v); // this one is column order ! - EQUALITY(A,trans(B)); + double data[] = { -5, 3, 0, -5 }; + Matrix A = Matrix_(2, 2, data); + Vector v(4); + copy(data, data + 4, v.data()); + Matrix B = Matrix_(2, 2, v); // this one is column order ! + EQUALITY(A,trans(B)); } /* ************************************************************************* */ TEST( matrix, Matrix_ ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B(2, 2); - B(0, 0) = -5; - B(0, 1) = 3; - B(1, 0) = 0; - B(1, 1) = -5; + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B(2, 2); + B(0, 0) = -5; + B(0, 1) = 3; + B(1, 0) = 0; + B(1, 1) = -5; - EQUALITY(A,B); + EQUALITY(A,B); } /* ************************************************************************* */ TEST( matrix, col_major ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - const double * const a = &A(0, 0); - EXPECT_DOUBLES_EQUAL(1, a[0], tol); - EXPECT_DOUBLES_EQUAL(3, a[1], tol); - EXPECT_DOUBLES_EQUAL(2, a[2], tol); - EXPECT_DOUBLES_EQUAL(4, a[3], tol); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + const double * const a = &A(0, 0); + EXPECT_DOUBLES_EQUAL(1, a[0], tol); + EXPECT_DOUBLES_EQUAL(3, a[1], tol); + EXPECT_DOUBLES_EQUAL(2, a[2], tol); + EXPECT_DOUBLES_EQUAL(4, a[3], tol); } /* ************************************************************************* */ TEST( matrix, collect1 ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); - Matrix AB = collect(2, &A, &B); - Matrix C(2, 5); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 2; j++) - C(i, j) = A(i, j); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 3; j++) - C(i, j + 2) = B(i, j); + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix AB = collect(2, &A, &B); + Matrix C(2, 5); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 2; j++) + C(i, j) = A(i, j); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 3; j++) + C(i, j + 2) = B(i, j); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ TEST( matrix, collect2 ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); - vector matrices; - matrices.push_back(&A); - matrices.push_back(&B); - Matrix AB = collect(matrices); - Matrix C(2, 5); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 2; j++) - C(i, j) = A(i, j); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 3; j++) - C(i, j + 2) = B(i, j); + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + vector matrices; + matrices.push_back(&A); + matrices.push_back(&B); + Matrix AB = collect(matrices); + Matrix C(2, 5); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 2; j++) + C(i, j) = A(i, j); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 3; j++) + C(i, j + 2) = B(i, j); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ TEST( matrix, collect3 ) { - Matrix A, B; - A = eye(2, 3); - B = eye(2, 3); - vector matrices; - matrices.push_back(&A); - matrices.push_back(&B); - Matrix AB = collect(matrices, 2, 3); - Matrix exp = Matrix_(2, 6, - 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 1.0, 0.0); + Matrix A, B; + A = eye(2, 3); + B = eye(2, 3); + vector matrices; + matrices.push_back(&A); + matrices.push_back(&B); + Matrix AB = collect(matrices, 2, 3); + Matrix exp = Matrix_(2, 6, + 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 1.0, 0.0); - EQUALITY(exp,AB); + EQUALITY(exp,AB); } /* ************************************************************************* */ TEST( matrix, stack ) { - Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); - Matrix B = Matrix_(3, 2, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); - Matrix AB = stack(2, &A, &B); - Matrix C(5, 2); - for (int i = 0; i < 2; i++) - for (int j = 0; j < 2; j++) - C(i, j) = A(i, j); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 2; j++) - C(i + 2, j) = B(i, j); + Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0); + Matrix B = Matrix_(3, 2, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); + Matrix AB = stack(2, &A, &B); + Matrix C(5, 2); + for (int i = 0; i < 2; i++) + for (int j = 0; j < 2; j++) + C(i, j) = A(i, j); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 2; j++) + C(i + 2, j) = B(i, j); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ TEST( matrix, column ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., - 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., - -0.1); - Vector a1 = column(A, 0); - Vector exp1 = Vector_(4, -1., 0., 1., 0.); - EXPECT(assert_equal(a1, exp1)); + Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., + -0.1); + Vector a1 = column(A, 0); + Vector exp1 = Vector_(4, -1., 0., 1., 0.); + EXPECT(assert_equal(a1, exp1)); - Vector a2 = column(A, 3); - Vector exp2 = Vector_(4, 0., 1., 0., 0.); - EXPECT(assert_equal(a2, exp2)); + Vector a2 = column(A, 3); + Vector exp2 = Vector_(4, 0., 1., 0., 0.); + EXPECT(assert_equal(a2, exp2)); - Vector a3 = column(A, 6); - Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1); - EXPECT(assert_equal(a3, exp3)); + Vector a3 = column(A, 6); + Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1); + EXPECT(assert_equal(a3, exp3)); } /* ************************************************************************* */ TEST( matrix, insert_column ) { - Matrix big = zeros(5, 6); - Vector col = ones(5); - size_t j = 3; + Matrix big = zeros(5, 6); + Vector col = ones(5); + size_t j = 3; - insertColumn(big, col, j); + insertColumn(big, col, j); - Matrix expected = Matrix_(5, 6, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); + Matrix expected = Matrix_(5, 6, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ TEST( matrix, insert_subcolumn ) { - Matrix big = zeros(5, 6); - Vector col1 = ones(2); - size_t i = 1; - size_t j = 3; + Matrix big = zeros(5, 6); + Vector col1 = ones(2); + size_t i = 1; + size_t j = 3; - insertColumn(big, col1, i, j); // check 1 + insertColumn(big, col1, i, j); // check 1 - Vector col2 = ones(1); - insertColumn(big, col2, 4, 5); // check 2 + Vector col2 = ones(1); + insertColumn(big, col2, 4, 5); // check 2 - Matrix expected = Matrix_(5, 6, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + Matrix expected = Matrix_(5, 6, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ TEST( matrix, row ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., - 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., - -0.1); - Vector a1 = row(A, 0); - Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); - EXPECT(assert_equal(a1, exp1)); + Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., + -0.1); + Vector a1 = row(A, 0); + Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); + EXPECT(assert_equal(a1, exp1)); - Vector a2 = row(A, 2); - Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); - EXPECT(assert_equal(a2, exp2)); + Vector a2 = row(A, 2); + Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); + EXPECT(assert_equal(a2, exp2)); - Vector a3 = row(A, 3); - Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); - EXPECT(assert_equal(a3, exp3)); + Vector a3 = row(A, 3); + Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); + EXPECT(assert_equal(a3, exp3)); } /* ************************************************************************* */ TEST( matrix, zeros ) { - Matrix A(2, 3); - A(0, 0) = 0; - A(0, 1) = 0; - A(0, 2) = 0; - A(1, 0) = 0; - A(1, 1) = 0; - A(1, 2) = 0; + Matrix A(2, 3); + A(0, 0) = 0; + A(0, 1) = 0; + A(0, 2) = 0; + A(1, 0) = 0; + A(1, 1) = 0; + A(1, 2) = 0; - Matrix zero = zeros(2, 3); + Matrix zero = zeros(2, 3); - EQUALITY(A , zero); + EQUALITY(A , zero); } /* ************************************************************************* */ TEST( matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = Matrix_(2, 3, 1.0, 1.0, 1.0, 1.0, 1.0, - 1.0); + Matrix big = zeros(5, 6), small = Matrix_(2, 3, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0); - insertSub(big, small, 1, 2); + insertSub(big, small, 1, 2); - Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ @@ -285,537 +285,537 @@ TEST( matrix, stream_read ) { /* ************************************************************************* */ TEST( matrix, scale_columns ) { - Matrix A(3, 4); - A(0, 0) = 1.; - A(0, 1) = 1.; - A(0, 2) = 1.; - A(0, 3) = 1.; - A(1, 0) = 1.; - A(1, 1) = 1.; - A(1, 2) = 1.; - A(1, 3) = 1.; - A(2, 0) = 1.; - A(2, 1) = 1.; - A(2, 2) = 1.; - A(2, 3) = 1.; + Matrix A(3, 4); + A(0, 0) = 1.; + A(0, 1) = 1.; + A(0, 2) = 1.; + A(0, 3) = 1.; + A(1, 0) = 1.; + A(1, 1) = 1.; + A(1, 2) = 1.; + A(1, 3) = 1.; + A(2, 0) = 1.; + A(2, 1) = 1.; + A(2, 2) = 1.; + A(2, 3) = 1.; - Vector v = Vector_(4, 2., 3., 4., 5.); + Vector v = Vector_(4, 2., 3., 4., 5.); - Matrix actual = vector_scale(A, v); + Matrix actual = vector_scale(A, v); - Matrix expected(3, 4); - expected(0, 0) = 2.; - expected(0, 1) = 3.; - expected(0, 2) = 4.; - expected(0, 3) = 5.; - expected(1, 0) = 2.; - expected(1, 1) = 3.; - expected(1, 2) = 4.; - expected(1, 3) = 5.; - expected(2, 0) = 2.; - expected(2, 1) = 3.; - expected(2, 2) = 4.; - expected(2, 3) = 5.; + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 3.; + expected(0, 2) = 4.; + expected(0, 3) = 5.; + expected(1, 0) = 2.; + expected(1, 1) = 3.; + expected(1, 2) = 4.; + expected(1, 3) = 5.; + expected(2, 0) = 2.; + expected(2, 1) = 3.; + expected(2, 2) = 4.; + expected(2, 3) = 5.; - EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actual, expected)); } /* ************************************************************************* */ TEST( matrix, scale_rows ) { - Matrix A(3, 4); - A(0, 0) = 1.; - A(0, 1) = 1.; - A(0, 2) = 1.; - A(0, 3) = 1.; - A(1, 0) = 1.; - A(1, 1) = 1.; - A(1, 2) = 1.; - A(1, 3) = 1.; - A(2, 0) = 1.; - A(2, 1) = 1.; - A(2, 2) = 1.; - A(2, 3) = 1.; + Matrix A(3, 4); + A(0, 0) = 1.; + A(0, 1) = 1.; + A(0, 2) = 1.; + A(0, 3) = 1.; + A(1, 0) = 1.; + A(1, 1) = 1.; + A(1, 2) = 1.; + A(1, 3) = 1.; + A(2, 0) = 1.; + A(2, 1) = 1.; + A(2, 2) = 1.; + A(2, 3) = 1.; - Vector v = Vector_(3, 2., 3., 4.); + Vector v = Vector_(3, 2., 3., 4.); - Matrix actual = vector_scale(v, A); + Matrix actual = vector_scale(v, A); - Matrix expected(3, 4); - expected(0, 0) = 2.; - expected(0, 1) = 2.; - expected(0, 2) = 2.; - expected(0, 3) = 2.; - expected(1, 0) = 3.; - expected(1, 1) = 3.; - expected(1, 2) = 3.; - expected(1, 3) = 3.; - expected(2, 0) = 4.; - expected(2, 1) = 4.; - expected(2, 2) = 4.; - expected(2, 3) = 4.; + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 2.; + expected(0, 2) = 2.; + expected(0, 3) = 2.; + expected(1, 0) = 3.; + expected(1, 1) = 3.; + expected(1, 2) = 3.; + expected(1, 3) = 3.; + expected(2, 0) = 4.; + expected(2, 1) = 4.; + expected(2, 2) = 4.; + expected(2, 3) = 4.; - EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actual, expected)); } /* ************************************************************************* */ TEST( matrix, scale_rows_mask ) { - Matrix A(3, 4); - A(0, 0) = 1.; - A(0, 1) = 1.; - A(0, 2) = 1.; - A(0, 3) = 1.; - A(1, 0) = 1.; - A(1, 1) = 1.; - A(1, 2) = 1.; - A(1, 3) = 1.; - A(2, 0) = 1.; - A(2, 1) = 1.; - A(2, 2) = 1.; - A(2, 3) = 1.; + Matrix A(3, 4); + A(0, 0) = 1.; + A(0, 1) = 1.; + A(0, 2) = 1.; + A(0, 3) = 1.; + A(1, 0) = 1.; + A(1, 1) = 1.; + A(1, 2) = 1.; + A(1, 3) = 1.; + A(2, 0) = 1.; + A(2, 1) = 1.; + A(2, 2) = 1.; + A(2, 3) = 1.; - Vector v = Vector_(3, 2., std::numeric_limits::infinity(), 4.); + Vector v = Vector_(3, 2., std::numeric_limits::infinity(), 4.); - Matrix actual = vector_scale(v, A, true); + Matrix actual = vector_scale(v, A, true); - Matrix expected(3, 4); - expected(0, 0) = 2.; - expected(0, 1) = 2.; - expected(0, 2) = 2.; - expected(0, 3) = 2.; - expected(1, 0) = 1.; - expected(1, 1) = 1.; - expected(1, 2) = 1.; - expected(1, 3) = 1.; - expected(2, 0) = 4.; - expected(2, 1) = 4.; - expected(2, 2) = 4.; - expected(2, 3) = 4.; + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 2.; + expected(0, 2) = 2.; + expected(0, 3) = 2.; + expected(1, 0) = 1.; + expected(1, 1) = 1.; + expected(1, 2) = 1.; + expected(1, 3) = 1.; + expected(2, 0) = 4.; + expected(2, 1) = 4.; + expected(2, 2) = 4.; + expected(2, 3) = 4.; - EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actual, expected)); } /* ************************************************************************* */ TEST( matrix, equal ) { - Matrix A(4, 4); - A(0, 0) = -1; - A(0, 1) = 1; - A(0, 2) = 2; - A(0, 3) = 3; - A(1, 0) = 1; - A(1, 1) = -3; - A(1, 2) = 1; - A(1, 3) = 3; - A(2, 0) = 1; - A(2, 1) = 2; - A(2, 2) = -1; - A(2, 3) = 4; - A(3, 0) = 2; - A(3, 1) = 1; - A(3, 2) = 2; - A(3, 3) = -2; + Matrix A(4, 4); + A(0, 0) = -1; + A(0, 1) = 1; + A(0, 2) = 2; + A(0, 3) = 3; + A(1, 0) = 1; + A(1, 1) = -3; + A(1, 2) = 1; + A(1, 3) = 3; + A(2, 0) = 1; + A(2, 1) = 2; + A(2, 2) = -1; + A(2, 3) = 4; + A(3, 0) = 2; + A(3, 1) = 1; + A(3, 2) = 2; + A(3, 3) = -2; - Matrix A2(A); + Matrix A2(A); - Matrix A3(A); - A3(3, 3) = -2.1; + Matrix A3(A); + A3(3, 3) = -2.1; - EXPECT(A==A2); - EXPECT(A!=A3); + EXPECT(A==A2); + EXPECT(A!=A3); } /* ************************************************************************* */ TEST( matrix, equal_nan ) { - Matrix A(4, 4); - A(0, 0) = -1; - A(0, 1) = 1; - A(0, 2) = 2; - A(0, 3) = 3; - A(1, 0) = 1; - A(1, 1) = -3; - A(1, 2) = 1; - A(1, 3) = 3; - A(2, 0) = 1; - A(2, 1) = 2; - A(2, 2) = -1; - A(2, 3) = 4; - A(3, 0) = 2; - A(3, 1) = 1; - A(3, 2) = 2; - A(3, 3) = -2; + Matrix A(4, 4); + A(0, 0) = -1; + A(0, 1) = 1; + A(0, 2) = 2; + A(0, 3) = 3; + A(1, 0) = 1; + A(1, 1) = -3; + A(1, 2) = 1; + A(1, 3) = 3; + A(2, 0) = 1; + A(2, 1) = 2; + A(2, 2) = -1; + A(2, 3) = 4; + A(3, 0) = 2; + A(3, 1) = 1; + A(3, 2) = 2; + A(3, 3) = -2; - Matrix A2(A); + Matrix A2(A); - Matrix A3(A); - A3(3, 3) = inf; + Matrix A3(A); + A3(3, 3) = inf; - EXPECT(A!=A3); + EXPECT(A!=A3); } /* ************************************************************************* */ TEST( matrix, addition ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); - EQUALITY(A+B,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); + EQUALITY(A+B,C); } /* ************************************************************************* */ TEST( matrix, addition_in_place ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); - A += B; - EQUALITY(A,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0); + A += B; + EQUALITY(A,C); } /* ************************************************************************* */ TEST( matrix, subtraction ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); - EQUALITY(A-B,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); + EQUALITY(A-B,C); } /* ************************************************************************* */ TEST( matrix, subtraction_in_place ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); - Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); - A -= B; - EQUALITY(A,C); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0); + Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0); + A -= B; + EQUALITY(A,C); } /* ************************************************************************* */ TEST( matrix, multiplication ) { - Matrix A(2, 2); - A(0, 0) = -1; - A(1, 0) = 1; - A(0, 1) = 1; - A(1, 1) = -3; + Matrix A(2, 2); + A(0, 0) = -1; + A(1, 0) = 1; + A(0, 1) = 1; + A(1, 1) = -3; - Matrix B(2, 1); - B(0, 0) = 1.2; - B(1, 0) = 3.4; + Matrix B(2, 1); + B(0, 0) = 1.2; + B(1, 0) = 3.4; - Matrix AB(2, 1); - AB(0, 0) = 2.2; - AB(1, 0) = -9.; + Matrix AB(2, 1); + AB(0, 0) = 2.2; + AB(1, 0) = -9.; - EQUALITY(A*B,AB); + EQUALITY(A*B,AB); } /* ************************************************************************* */ TEST( matrix, scalar_matrix_multiplication ) { - Vector result(2); + Vector result(2); - Matrix A(2, 2); - A(0, 0) = -1; - A(1, 0) = 1; - A(0, 1) = 1; - A(1, 1) = -3; + Matrix A(2, 2); + A(0, 0) = -1; + A(1, 0) = 1; + A(0, 1) = 1; + A(1, 1) = -3; - Matrix B(2, 2); - B(0, 0) = -10; - B(1, 0) = 10; - B(0, 1) = 10; - B(1, 1) = -30; + Matrix B(2, 2); + B(0, 0) = -10; + B(1, 0) = 10; + B(0, 1) = 10; + B(1, 1) = -30; - EQUALITY((10*A),B); + EQUALITY((10*A),B); } /* ************************************************************************* */ TEST( matrix, matrix_vector_multiplication ) { - Vector result(2); + Vector result(2); - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Vector v = Vector_(3, 1., 2., 3.); - Vector Av = Vector_(2, 14., 32.); - Vector AtAv = Vector_(3, 142., 188., 234.); + Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Vector v = Vector_(3, 1., 2., 3.); + Vector Av = Vector_(2, 14., 32.); + Vector AtAv = Vector_(3, 142., 188., 234.); - EQUALITY(A*v,Av); - EQUALITY(A^Av,AtAv); + EQUALITY(A*v,Av); + EQUALITY(A^Av,AtAv); } /* ************************************************************************* */ TEST( matrix, nrRowsAndnrCols ) { - Matrix A(3, 6); - LONGS_EQUAL( A.rows() , 3 ); - LONGS_EQUAL( A.cols() , 6 ); + Matrix A(3, 6); + LONGS_EQUAL( A.rows() , 3 ); + LONGS_EQUAL( A.cols() , 6 ); } /* ************************************************************************* */ TEST( matrix, scalar_divide ) { - Matrix A(2, 2); - A(0, 0) = 10; - A(1, 0) = 30; - A(0, 1) = 20; - A(1, 1) = 40; + Matrix A(2, 2); + A(0, 0) = 10; + A(1, 0) = 30; + A(0, 1) = 20; + A(1, 1) = 40; - Matrix B(2, 2); - B(0, 0) = 1; - B(1, 0) = 3; - B(0, 1) = 2; - B(1, 1) = 4; + Matrix B(2, 2); + B(0, 0) = 1; + B(1, 0) = 3; + B(0, 1) = 2; + B(1, 1) = 4; - EQUALITY(B,A/10); + EQUALITY(B,A/10); } /* ************************************************************************* */ TEST( matrix, zero_below_diagonal ) { - Matrix A1 = Matrix_(3, 4, - 1.0, 2.0, 3.0, 4.0, - 1.0, 2.0, 3.0, 4.0, - 1.0, 2.0, 3.0, 4.0); + Matrix A1 = Matrix_(3, 4, + 1.0, 2.0, 3.0, 4.0, + 1.0, 2.0, 3.0, 4.0, + 1.0, 2.0, 3.0, 4.0); - Matrix expected1 = Matrix_(3, 4, - 1.0, 2.0, 3.0, 4.0, - 0.0, 2.0, 3.0, 4.0, - 0.0, 0.0, 3.0, 4.0); - Matrix actual1r = A1; - zeroBelowDiagonal(actual1r); - EXPECT(assert_equal(expected1, actual1r, 1e-10)); + Matrix expected1 = Matrix_(3, 4, + 1.0, 2.0, 3.0, 4.0, + 0.0, 2.0, 3.0, 4.0, + 0.0, 0.0, 3.0, 4.0); + Matrix actual1r = A1; + zeroBelowDiagonal(actual1r); + EXPECT(assert_equal(expected1, actual1r, 1e-10)); - Matrix actual1c = A1; - zeroBelowDiagonal(actual1c); - EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); + Matrix actual1c = A1; + zeroBelowDiagonal(actual1c); + EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - actual1c = A1; - zeroBelowDiagonal(actual1c, 4); - EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); + actual1c = A1; + zeroBelowDiagonal(actual1c, 4); + EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - Matrix A2 = Matrix_(5, 3, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0, - 1.0, 2.0, 3.0); - Matrix expected2 = Matrix_(5, 3, - 1.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 0.0, 3.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); + Matrix A2 = Matrix_(5, 3, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0, + 1.0, 2.0, 3.0); + Matrix expected2 = Matrix_(5, 3, + 1.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 0.0, 3.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0); - Matrix actual2r = A2; - zeroBelowDiagonal(actual2r); - EXPECT(assert_equal(expected2, actual2r, 1e-10)); + Matrix actual2r = A2; + zeroBelowDiagonal(actual2r); + EXPECT(assert_equal(expected2, actual2r, 1e-10)); - Matrix actual2c = A2; - zeroBelowDiagonal(actual2c); - EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); + Matrix actual2c = A2; + zeroBelowDiagonal(actual2c); + EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); - Matrix expected2_partial = Matrix_(5, 3, - 1.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0, - 0.0, 2.0, 3.0); - actual2c = A2; - zeroBelowDiagonal(actual2c, 1); - EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); + Matrix expected2_partial = Matrix_(5, 3, + 1.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 2.0, 3.0, + 0.0, 2.0, 3.0); + actual2c = A2; + zeroBelowDiagonal(actual2c, 1); + EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10)); } /* ************************************************************************* */ TEST( matrix, inverse ) { - Matrix A(3, 3); - A(0, 0) = 1; - A(0, 1) = 2; - A(0, 2) = 3; - A(1, 0) = 0; - A(1, 1) = 4; - A(1, 2) = 5; - A(2, 0) = 1; - A(2, 1) = 0; - A(2, 2) = 6; + Matrix A(3, 3); + A(0, 0) = 1; + A(0, 1) = 2; + A(0, 2) = 3; + A(1, 0) = 0; + A(1, 1) = 4; + A(1, 2) = 5; + A(2, 0) = 1; + A(2, 1) = 0; + A(2, 2) = 6; - Matrix Ainv = inverse(A); - EXPECT(assert_equal(eye(3), A*Ainv)); - EXPECT(assert_equal(eye(3), Ainv*A)); + Matrix Ainv = inverse(A); + EXPECT(assert_equal(eye(3), A*Ainv)); + EXPECT(assert_equal(eye(3), Ainv*A)); - Matrix expected(3, 3); - expected(0, 0) = 1.0909; - expected(0, 1) = -0.5454; - expected(0, 2) = -0.0909; - expected(1, 0) = 0.2272; - expected(1, 1) = 0.1363; - expected(1, 2) = -0.2272; - expected(2, 0) = -0.1818; - expected(2, 1) = 0.0909; - expected(2, 2) = 0.1818; + Matrix expected(3, 3); + expected(0, 0) = 1.0909; + expected(0, 1) = -0.5454; + expected(0, 2) = -0.0909; + expected(1, 0) = 0.2272; + expected(1, 1) = 0.1363; + expected(1, 2) = -0.2272; + expected(2, 0) = -0.1818; + expected(2, 1) = 0.0909; + expected(2, 2) = 0.1818; - EXPECT(assert_equal(expected, Ainv, 1e-4)); + EXPECT(assert_equal(expected, Ainv, 1e-4)); - // These two matrices failed before version 2003 because we called LU incorrectly - Matrix lMg(Matrix_(3, 3, 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal(Matrix_(3,3, - 0.0, -1.0, 1.0, - 1.0, 0.0, 2.0, - 0.0, 0.0, 1.0), - inverse(lMg))); - Matrix gMl(Matrix_(3, 3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); - EXPECT(assert_equal(Matrix_(3,3, - 0.0, 1.0,-2.0, - -1.0, 0.0, 1.0, - 0.0, 0.0, 1.0), - inverse(gMl))); + // These two matrices failed before version 2003 because we called LU incorrectly + Matrix lMg(Matrix_(3, 3, 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal(Matrix_(3,3, + 0.0, -1.0, 1.0, + 1.0, 0.0, 2.0, + 0.0, 0.0, 1.0), + inverse(lMg))); + Matrix gMl(Matrix_(3, 3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal(Matrix_(3,3, + 0.0, 1.0,-2.0, + -1.0, 0.0, 1.0, + 0.0, 0.0, 1.0), + inverse(gMl))); } /* ************************************************************************* */ TEST( matrix, inverse2 ) { - Matrix A(3, 3); - A(0, 0) = 0; - A(0, 1) = -1; - A(0, 2) = 1; - A(1, 0) = 1; - A(1, 1) = 0; - A(1, 2) = 2; - A(2, 0) = 0; - A(2, 1) = 0; - A(2, 2) = 1; + Matrix A(3, 3); + A(0, 0) = 0; + A(0, 1) = -1; + A(0, 2) = 1; + A(1, 0) = 1; + A(1, 1) = 0; + A(1, 2) = 2; + A(2, 0) = 0; + A(2, 1) = 0; + A(2, 2) = 1; - Matrix Ainv = inverse(A); + Matrix Ainv = inverse(A); - Matrix expected(3, 3); - expected(0, 0) = 0; - expected(0, 1) = 1; - expected(0, 2) = -2; - expected(1, 0) = -1; - expected(1, 1) = 0; - expected(1, 2) = 1; - expected(2, 0) = 0; - expected(2, 1) = 0; - expected(2, 2) = 1; + Matrix expected(3, 3); + expected(0, 0) = 0; + expected(0, 1) = 1; + expected(0, 2) = -2; + expected(1, 0) = -1; + expected(1, 1) = 0; + expected(1, 2) = 1; + expected(2, 0) = 0; + expected(2, 1) = 0; + expected(2, 2) = 1; - EXPECT(assert_equal(expected, Ainv, 1e-4)); + EXPECT(assert_equal(expected, Ainv, 1e-4)); } /* ************************************************************************* */ TEST( matrix, backsubtitution ) { - // TEST ONE 2x2 matrix U1*x=b1 - Vector expected1 = Vector_(2, 3.6250, -0.75); - Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.); - Vector b1 = U22 * expected1; - EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); + // TEST ONE 2x2 matrix U1*x=b1 + Vector expected1 = Vector_(2, 3.6250, -0.75); + Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.); + Vector b1 = U22 * expected1; + EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); - // TEST TWO 3x3 matrix U2*x=b2 - Vector expected2 = Vector_(3, 5.5, -8.5, 5.); - Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.); - Vector b2 = U33 * expected2; - EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); + // TEST TWO 3x3 matrix U2*x=b2 + Vector expected2 = Vector_(3, 5.5, -8.5, 5.); + Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.); + Vector b2 = U33 * expected2; + EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); - // TEST THREE Lower triangular 3x3 matrix L3*x=b3 - Vector expected3 = Vector_(3, 1., 1., 1.); - Matrix L3 = trans(U33); - Vector b3 = L3 * expected3; - EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); + // TEST THREE Lower triangular 3x3 matrix L3*x=b3 + Vector expected3 = Vector_(3, 1., 1., 1.); + Matrix L3 = trans(U33); + Vector b3 = L3 * expected3; + EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); - // TEST FOUR Try the above with transpose backSubstituteUpper - EXPECT( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001)); + // TEST FOUR Try the above with transpose backSubstituteUpper + EXPECT( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001)); } /* ************************************************************************* */ TEST( matrix, householder ) { - double data[] = { -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; + double data[] = { -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, - 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, - -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, - 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1 = Matrix_(4, 7, data1); - Matrix A1 = Matrix_(4, 7, data); - householder_(A1, 3); - EXPECT(assert_equal(expected1, A1, 1e-3)); + // check in-place householder, with v vectors below diagonal + double data1[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected1 = Matrix_(4, 7, data1); + Matrix A1 = Matrix_(4, 7, data); + householder_(A1, 3); + EXPECT(assert_equal(expected1, A1, 1e-3)); - // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, - 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected = Matrix_(4, 7, data2); - Matrix A2 = Matrix_(4, 7, data); - householder(A2, 3); - EXPECT(assert_equal(expected, A2, 1e-3)); + // in-place, with zeros below diagonal + double data2[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, + 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected = Matrix_(4, 7, data2); + Matrix A2 = Matrix_(4, 7, data); + householder(A2, 3); + EXPECT(assert_equal(expected, A2, 1e-3)); } /* ************************************************************************* */ TEST( matrix, householder_colMajor ) { - double data[] = { - -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; + double data[] = { + -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, - 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, - -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, - 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1(Matrix_(4, 7, data1)); - Matrix A1(Matrix_(4, 7, data)); - householder_(A1, 3); - EXPECT(assert_equal(expected1, A1, 1e-3)); + // check in-place householder, with v vectors below diagonal + double data1[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + -0.618034, 0, 4.4721, 0, -4.4721, 0, 0, + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected1(Matrix_(4, 7, data1)); + Matrix A1(Matrix_(4, 7, data)); + householder_(A1, 3); + EXPECT(assert_equal(expected1, A1, 1e-3)); - // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, - 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected(Matrix_(4, 7, data2)); - Matrix A2(Matrix_(4, 7, data)); - householder(A2, 3); - EXPECT(assert_equal(expected, A2, 1e-3)); + // in-place, with zeros below diagonal + double data2[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, + 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected(Matrix_(4, 7, data2)); + Matrix A2(Matrix_(4, 7, data)); + householder(A2, 3); + EXPECT(assert_equal(expected, A2, 1e-3)); } /* ************************************************************************* */ TEST( matrix, eigen_QR ) { - // use standard Eigen function to yield a non-in-place QR factorization - double data[] = { - -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; + // use standard Eigen function to yield a non-in-place QR factorization + double data[] = { + -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; - // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, - 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, - 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected(Matrix_(4, 7, data2)); - Matrix A(Matrix_(4, 7, data)); - Matrix actual = A.householderQr().matrixQR(); - zeroBelowDiagonal(actual); + // in-place, with zeros below diagonal + double data2[] = { + 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0, + 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected(Matrix_(4, 7, data2)); + Matrix A(Matrix_(4, 7, data)); + Matrix actual = A.householderQr().matrixQR(); + zeroBelowDiagonal(actual); - EXPECT(assert_equal(expected, actual, 1e-3)); + EXPECT(assert_equal(expected, actual, 1e-3)); - // use shiny new in place QR inside gtsam - A = Matrix(Matrix_(4, 7, data)); - inplace_QR(A); - EXPECT(assert_equal(expected, A, 1e-3)); + // use shiny new in place QR inside gtsam + A = Matrix(Matrix_(4, 7, data)); + inplace_QR(A); + EXPECT(assert_equal(expected, A, 1e-3)); } /* ************************************************************************* */ @@ -824,125 +824,125 @@ TEST( matrix, eigen_QR ) /* ************************************************************************* */ TEST( matrix, qr ) { - double data[] = { -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, - 0, 0, -10, 10, 0, -10, 0 }; - Matrix A = Matrix_(6, 4, data); + double data[] = { -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, + 0, 0, -10, 10, 0, -10, 0 }; + Matrix A = Matrix_(6, 4, data); - double dataQ[] = { -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, - 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944, - 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667, - 0, -0.5963, 0, 0, -0.4472, }; - Matrix expectedQ = Matrix_(6, 6, dataQ); + double dataQ[] = { -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0, + 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944, + 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667, + 0, -0.5963, 0, 0, -0.4472, }; + Matrix expectedQ = Matrix_(6, 6, dataQ); - double dataR[] = { 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, - 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0, }; - Matrix expectedR = Matrix_(6, 4, dataR); + double dataR[] = { 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0, + 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0, }; + Matrix expectedR = Matrix_(6, 4, dataR); - Matrix Q, R; - boost::tie(Q, R) = qr(A); - EXPECT(assert_equal(expectedQ, Q, 1e-4)); - EXPECT(assert_equal(expectedR, R, 1e-4)); - EXPECT(assert_equal(A, Q*R, 1e-14)); + Matrix Q, R; + boost::tie(Q, R) = qr(A); + EXPECT(assert_equal(expectedQ, Q, 1e-4)); + EXPECT(assert_equal(expectedR, R, 1e-4)); + EXPECT(assert_equal(A, Q*R, 1e-14)); } /* ************************************************************************* */ TEST( matrix, sub ) { - double data1[] = { -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, - 0, 00, 10, 0, 0, 0, -10 }; - Matrix A = Matrix_(4, 6, data1); - Matrix actual = sub(A, 1, 3, 1, 5); + double data1[] = { -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, + 0, 00, 10, 0, 0, 0, -10 }; + Matrix A = Matrix_(4, 6, data1); + Matrix actual = sub(A, 1, 3, 1, 5); - double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; - Matrix expected = Matrix_(2, 4, data2); + double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; + Matrix expected = Matrix_(2, 4, data2); - EQUALITY(actual,expected); + EQUALITY(actual,expected); } /* ************************************************************************* */ TEST( matrix, trans ) { - Matrix A = Matrix_(2, 2, 1.0, 3.0, 2.0, 4.0); - Matrix B = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - EQUALITY(trans(A),B); + Matrix A = Matrix_(2, 2, 1.0, 3.0, 2.0, 4.0); + Matrix B = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + EQUALITY(trans(A),B); } /* ************************************************************************* */ TEST( matrix, col_major_access ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); - const double* a = &A(0, 0); - DOUBLES_EQUAL(2.0,a[2],1e-9); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + const double* a = &A(0, 0); + DOUBLES_EQUAL(2.0,a[2],1e-9); } /* ************************************************************************* */ TEST( matrix, weighted_elimination ) { - // create a matrix to eliminate - Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., - 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.); - Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); - Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + // create a matrix to eliminate + Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., + 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.); + Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); + Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); - // expected values - Matrix expectedR = Matrix_(4, 6, 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., - -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.); - Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); - Vector newSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); + // expected values + Matrix expectedR = Matrix_(4, 6, 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0., + -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.); + Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); + Vector newSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); - Vector r; - double di, sigma; - size_t i; + Vector r; + double di, sigma; + size_t i; - // perform elimination - Matrix A1 = A; - Vector b1 = b; - std::list > solution = - weighted_eliminate(A1, b1, sigmas); + // perform elimination + Matrix A1 = A; + Vector b1 = b; + std::list > solution = + weighted_eliminate(A1, b1, sigmas); - // unpack and verify - i = 0; - BOOST_FOREACH(boost::tie(r, di, sigma), solution){ - EXPECT(assert_equal(r, expectedR.row(i))); // verify r - DOUBLES_EQUAL(d(i), di, 1e-8); // verify d - DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma - i += 1; - } + // unpack and verify + i = 0; + BOOST_FOREACH(boost::tie(r, di, sigma), solution){ + EXPECT(assert_equal(r, expectedR.row(i))); // verify r + DOUBLES_EQUAL(d(i), di, 1e-8); // verify d + DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma + i += 1; + } } /* ************************************************************************* */ TEST( matrix, inverse_square_root ) { - Matrix measurement_covariance = Matrix_(3, 3, 0.25, 0.0, 0.0, 0.0, 0.25, - 0.0, 0.0, 0.0, 0.01); - Matrix actual = inverse_square_root(measurement_covariance); + Matrix measurement_covariance = Matrix_(3, 3, 0.25, 0.0, 0.0, 0.0, 0.25, + 0.0, 0.0, 0.0, 0.01); + Matrix actual = inverse_square_root(measurement_covariance); - Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, - 10.0); + Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, + 10.0); - EQUALITY(expected,actual); - EQUALITY(measurement_covariance,inverse(actual*actual)); + EQUALITY(expected,actual); + EQUALITY(measurement_covariance,inverse(actual*actual)); - // Randomly generated test. This test really requires inverse to - // be working well; if it's not, there's the possibility of a - // bug in inverse masking a bug in this routine since we - // use the same inverse routing inside inverse_square_root() - // as we use here to check it. + // Randomly generated test. This test really requires inverse to + // be working well; if it's not, there's the possibility of a + // bug in inverse masking a bug in this routine since we + // use the same inverse routing inside inverse_square_root() + // as we use here to check it. - Matrix M = Matrix_(5, 5, - 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, - 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, - -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, - -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, - 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); + Matrix M = Matrix_(5, 5, + 0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, + 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, + -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, + -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468, + 0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517); - expected = Matrix_(5, 5, - 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, - 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, - -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); - EQUALITY(expected, inverse_square_root(M)); + expected = Matrix_(5, 5, + 3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, + 0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, + -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); + EQUALITY(expected, inverse_square_root(M)); } @@ -951,80 +951,80 @@ TEST( matrix, inverse_square_root ) // we are checking against was generated via chol(M)' on octave TEST( matrix, LLt ) { - Matrix M = Matrix_(5, 5, 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + Matrix M = Matrix_(5, 5, 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); - Matrix expected = Matrix_(5, 5, - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); + Matrix expected = Matrix_(5, 5, + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - EQUALITY(expected, LLt(M)); + EQUALITY(expected, LLt(M)); } /* ************************************************************************* */ TEST( matrix, multiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), - expected = e + A * x; + Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), + expected = e + A * x; - multiplyAdd(1, A, x, e); - EXPECT(assert_equal(expected, e)); + multiplyAdd(1, A, x, e); + EXPECT(assert_equal(expected, e)); } /* ************************************************************************* */ TEST( matrix, transposeMultiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); - Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), - expected = x + trans(A) * e; + Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), + expected = x + trans(A) * e; - transposeMultiplyAdd(1, A, e, x); - EXPECT(assert_equal(expected, x)); + transposeMultiplyAdd(1, A, e, x); + EXPECT(assert_equal(expected, x)); } /* ************************************************************************* */ TEST( matrix, linear_dependent ) { - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); - EXPECT(linear_dependent(A, B)); + Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = Matrix_(2, 3, -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); + EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent2 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); - EXPECT(linear_dependent(A, B)); + Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); + EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent3 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); - EXPECT(linear_independent(A, B)); + Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); + EXPECT(linear_independent(A, B)); } /* ************************************************************************* */ TEST( matrix, svd1 ) { - Vector v = Vector_(3, 2., 1., 0.); - Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) - * Matrix(trans(V1)); - Matrix U, V; - Vector s; - svd(A, U, s, V); - Matrix S = diag(s); - EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); - EXPECT(assert_equal(S,S1)); + Vector v = Vector_(3, 2., 1., 0.); + Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) + * Matrix(trans(V1)); + Matrix U, V; + Vector s; + svd(A, U, s, V); + Matrix S = diag(s); + EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); + EXPECT(assert_equal(S,S1)); } /* ************************************************************************* */ @@ -1035,39 +1035,39 @@ static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ TEST( matrix, svd2 ) { - Matrix U, V; - Vector s; + Matrix U, V; + Vector s; - Matrix expectedU = Matrix_(3, 2, 0.,-1.,0.,0.,1.,0.); - Vector expected_s = Vector_(2, 3.,2.); - Matrix expectedV = Matrix_(2, 2, 1.,0.,0.,1.); + Matrix expectedU = Matrix_(3, 2, 0.,-1.,0.,0.,1.,0.); + Vector expected_s = Vector_(2, 3.,2.); + Matrix expectedV = Matrix_(2, 2, 1.,0.,0.,1.); - svd(sampleA, U, s, V); + svd(sampleA, U, s, V); - EXPECT(assert_equal(expectedU,U)); - EXPECT(assert_equal(expected_s,s,1e-9)); - EXPECT(assert_equal(expectedV,V)); + EXPECT(assert_equal(expectedU,U)); + EXPECT(assert_equal(expected_s,s,1e-9)); + EXPECT(assert_equal(expectedV,V)); } /* ************************************************************************* */ TEST( matrix, svd3 ) { - Matrix U, V; - Vector s; + Matrix U, V; + Vector s; - Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.); - Vector expected_s = Vector_(2, 3.0,2.0); - Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.); + Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.); + Vector expected_s = Vector_(2, 3.0,2.0); + Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.); - svd(sampleAt, U, s, V); - Matrix S = diag(s); - Matrix t = U * S; - Matrix Vt = trans(V); + svd(sampleAt, U, s, V); + Matrix S = diag(s); + Matrix t = U * S; + Matrix Vt = trans(V); - EXPECT(assert_equal(sampleAt, prod(t, Vt))); - EXPECT(assert_equal(expectedU,U)); - EXPECT(assert_equal(expected_s,s,1e-9)); - EXPECT(assert_equal(expectedV,V)); + EXPECT(assert_equal(sampleAt, prod(t, Vt))); + EXPECT(assert_equal(expectedU,U)); + EXPECT(assert_equal(expected_s,s,1e-9)); + EXPECT(assert_equal(expectedV,V)); } /* ************************************************************************* */ @@ -1126,7 +1126,7 @@ TEST( matrix, DLT ) /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index ffbaca368..d8c62b121 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -23,12 +23,12 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testTestableAssertions, optional ) { - typedef boost::optional OptionalScalar; - LieScalar x(1.0); - OptionalScalar ox(x), dummy = boost::none; - EXPECT(assert_equal(ox, ox)); - EXPECT(assert_equal(x, ox)); - EXPECT(assert_equal(dummy, dummy)); + typedef boost::optional OptionalScalar; + LieScalar x(1.0); + OptionalScalar ox(x), dummy = boost::none; + EXPECT(assert_equal(ox, ox)); + EXPECT(assert_equal(x, ox)); + EXPECT(assert_equal(dummy, dummy)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index b8bb0333b..8fd151dbf 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -101,15 +101,15 @@ TEST( TestVector, sub ) /* ************************************************************************* */ TEST( TestVector, subInsert ) { - Vector big = zero(6), - small = ones(3); + Vector big = zero(6), + small = ones(3); - size_t i = 2; - subInsert(big, small, i); + size_t i = 2; + subInsert(big, small, i); - Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); + Vector expected = Vector_(6, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0); - EXPECT(assert_equal(expected, big)); + EXPECT(assert_equal(expected, big)); } /* ************************************************************************* */ @@ -154,66 +154,66 @@ TEST( TestVector, concatVectors) /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse ) { - // column from a matrix - Vector x(2); - x(0) = 1.0; x(1) = 2.0; + // column from a matrix + Vector x(2); + x(0) = 1.0; x(1) = 2.0; - // create sigmas - Vector sigmas(2); - sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); + // create sigmas + Vector sigmas(2); + sigmas(0) = 0.1; sigmas(1) = 0.2; + Vector weights = reciprocal(emul(sigmas,sigmas)); - // perform solve - Vector actual; double precision; - boost::tie(actual, precision) = weightedPseudoinverse(x, weights); + // perform solve + Vector actual; double precision; + boost::tie(actual, precision) = weightedPseudoinverse(x, weights); - // construct expected - Vector expected(2); - expected(0) = 0.5; expected(1) = 0.25; - double expPrecision = 200.0; + // construct expected + Vector expected(2); + expected(0) = 0.5; expected(1) = 0.25; + double expPrecision = 200.0; - // verify - EXPECT(assert_equal(expected,actual)); - EXPECT(fabs(expPrecision-precision) < 1e-5); + // verify + EXPECT(assert_equal(expected,actual)); + EXPECT(fabs(expPrecision-precision) < 1e-5); } /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_constraint ) { - // column from a matrix - Vector x(2); - x(0) = 1.0; x(1) = 2.0; + // column from a matrix + Vector x(2); + x(0) = 1.0; x(1) = 2.0; - // create sigmas - Vector sigmas(2); - sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); + // create sigmas + Vector sigmas(2); + sigmas(0) = 0.0; sigmas(1) = 0.2; + Vector weights = reciprocal(emul(sigmas,sigmas)); - // perform solve - Vector actual; double precision; - boost::tie(actual, precision) = weightedPseudoinverse(x, weights); + // perform solve + Vector actual; double precision; + boost::tie(actual, precision) = weightedPseudoinverse(x, weights); - // construct expected - Vector expected(2); - expected(0) = 1.0; expected(1) = 0.0; + // construct expected + Vector expected(2); + expected(0) = 1.0; expected(1) = 0.0; - // verify - EXPECT(assert_equal(expected,actual)); - EXPECT(isinf(precision)); + // verify + EXPECT(assert_equal(expected,actual)); + EXPECT(isinf(precision)); } /* ************************************************************************* */ TEST( TestVector, weightedPseudoinverse_nan ) { - Vector a = Vector_(4, 1., 0., 0., 0.); - Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.); - Vector weights = reciprocal(emul(sigmas,sigmas)); - Vector pseudo; double precision; - boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); + Vector a = Vector_(4, 1., 0., 0., 0.); + Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.); + Vector weights = reciprocal(emul(sigmas,sigmas)); + Vector pseudo; double precision; + boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); - Vector expected = Vector_(4, 1., 0., 0.,0.); - EXPECT(assert_equal(expected, pseudo)); - DOUBLES_EQUAL(100, precision, 1e-5); + Vector expected = Vector_(4, 1., 0., 0.,0.); + EXPECT(assert_equal(expected, pseudo)); + DOUBLES_EQUAL(100, precision, 1e-5); } /* ************************************************************************* */ @@ -251,50 +251,50 @@ TEST( TestVector, axpy ) /* ************************************************************************* */ TEST( TestVector, equals ) { - Vector v1 = Vector_(1, 0.0/std::numeric_limits::quiet_NaN()); //testing nan - Vector v2 = Vector_(1, 1.0); - double tol = 1.; - EXPECT(!equal_with_abs_tol(v1, v2, tol)); + Vector v1 = Vector_(1, 0.0/std::numeric_limits::quiet_NaN()); //testing nan + Vector v2 = Vector_(1, 1.0); + double tol = 1.; + EXPECT(!equal_with_abs_tol(v1, v2, tol)); } /* ************************************************************************* */ TEST( TestVector, greater_than ) { - Vector v1 = Vector_(3, 1.0, 2.0, 3.0), - v2 = zero(3); - EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than - EXPECT(greaterThanOrEqual(v1, v2)); // test equals + Vector v1 = Vector_(3, 1.0, 2.0, 3.0), + v2 = zero(3); + EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than + EXPECT(greaterThanOrEqual(v1, v2)); // test equals } /* ************************************************************************* */ TEST( TestVector, reciprocal ) { - Vector v = Vector_(3, 1.0, 2.0, 4.0); + Vector v = Vector_(3, 1.0, 2.0, 4.0); EXPECT(assert_equal(Vector_(3, 1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ TEST( TestVector, linear_dependent ) { - Vector v1 = Vector_(3, 1.0, 2.0, 3.0); - Vector v2 = Vector_(3, -2.0, -4.0, -6.0); - EXPECT(linear_dependent(v1, v2)); + Vector v1 = Vector_(3, 1.0, 2.0, 3.0); + Vector v2 = Vector_(3, -2.0, -4.0, -6.0); + EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent2 ) { - Vector v1 = Vector_(3, 0.0, 2.0, 0.0); - Vector v2 = Vector_(3, 0.0, -4.0, 0.0); - EXPECT(linear_dependent(v1, v2)); + Vector v1 = Vector_(3, 0.0, 2.0, 0.0); + Vector v2 = Vector_(3, 0.0, -4.0, 0.0); + EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent3 ) { - Vector v1 = Vector_(3, 0.0, 2.0, 0.0); - Vector v2 = Vector_(3, 0.1, -4.1, 0.0); - EXPECT(!linear_dependent(v1, v2)); + Vector v1 = Vector_(3, 0.0, 2.0, 0.0); + Vector v2 = Vector_(3, 0.1, -4.1, 0.0); + EXPECT(!linear_dependent(v1, v2)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/timeMatrix.cpp b/gtsam/base/tests/timeMatrix.cpp index b85183854..9ed397ec8 100644 --- a/gtsam/base/tests/timeMatrix.cpp +++ b/gtsam/base/tests/timeMatrix.cpp @@ -36,41 +36,41 @@ using namespace gtsam; * - rev 2100 no pass: 18.45 sec , pass: 18.35 sec */ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { - // create a large number of matrices - // p = number of matrices - // m = rows per matrix - // n = columns per matrix - // reps = number of repetitions + // create a large number of matrices + // p = number of matrices + // m = rows per matrix + // n = columns per matrix + // reps = number of repetitions - // fill the matrices with identities - vector matrices; - for (size_t i=0; i matrices; + for (size_t i=0; i(M, j); - result = column(M, j); - elapsed = t.elapsed(); - } - return elapsed; + // extract a column + double elapsed; + Vector result; + { + boost::timer t; + for (size_t i=0; i(M, j); + result = column(M, j); + elapsed = t.elapsed(); + } + return elapsed; } /* @@ -188,24 +188,24 @@ double timeColumn(size_t reps) { * */ double timeHouseholder(size_t reps) { - // create a matrix - Matrix Abase = Matrix_(4, 7, - -5, 0, 5, 0, 0, 0, -1, - 00, -5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10, 0, 2, - 00, 10, 0, 0, 0,-10, -1); + // create a matrix + Matrix Abase = Matrix_(4, 7, + -5, 0, 5, 0, 0, 0, -1, + 00, -5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10, 0, 2, + 00, 10, 0, 0, 0,-10, -1); - // perform timing - double elapsed; - { - boost::timer t; - for (size_t i=0; i @@ -41,10 +41,10 @@ std::string timingPrefix; /* ************************************************************************* */ void TimingOutline::add(size_t usecs) { - t_ += usecs; - tIt_ += usecs; - t2_ += (double(usecs)/1000000.0)*(double(usecs)/1000000.0); - ++ n_; + t_ += usecs; + tIt_ += usecs; + t2_ += (double(usecs)/1000000.0)*(double(usecs)/1000000.0); + ++ n_; } /* ************************************************************************* */ @@ -53,38 +53,38 @@ TimingOutline::TimingOutline(const std::string& label) : /* ************************************************************************* */ size_t TimingOutline::time() const { - size_t time = 0; - bool hasChildren = false; - BOOST_FOREACH(const boost::shared_ptr& child, children_) { - if(child) { - time += child->time(); - hasChildren = true; - } - } - if(hasChildren) - return time; - else - return t_; + size_t time = 0; + bool hasChildren = false; + BOOST_FOREACH(const boost::shared_ptr& child, children_) { + if(child) { + time += child->time(); + hasChildren = true; + } + } + if(hasChildren) + return time; + else + return t_; } /* ************************************************************************* */ void TimingOutline::print(const std::string& outline) const { - std::cout << outline << " " << label_ << ": " << double(t_)/1000000.0 << " (" << - n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 << - " max: " << double(tMax_)/1000000.0 << ")\n"; - for(size_t i=0; i 0) - childOutline += "."; - childOutline += (boost::format("%d") % i).str(); + if(childOutline.size() > 0) + childOutline += "."; + childOutline += (boost::format("%d") % i).str(); #else - childOutline += " "; + childOutline += " "; #endif - children_[i]->print(childOutline); - } - } + children_[i]->print(childOutline); + } + } } void TimingOutline::print2(const std::string& outline, const double parentTotal) const { @@ -130,33 +130,33 @@ void TimingOutline::print2(const std::string& outline, const double parentTotal) /* ************************************************************************* */ const boost::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr) { - assert(thisPtr.lock().get() == this); - // Resize if necessary - if(child >= children_.size()) - children_.resize(child + 1); - // Create child if necessary - if(children_[child]) { + assert(thisPtr.lock().get() == this); + // Resize if necessary + if(child >= children_.size()) + children_.resize(child + 1); + // Create child if necessary + if(children_[child]) { #ifndef NDEBUG - if(children_[child]->label_ != label) { - timingRoot->print(); - std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl; - exit(1); - } + if(children_[child]->label_ != label) { + timingRoot->print(); + std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl; + exit(1); + } #endif - } else { - children_[child].reset(new TimingOutline(label)); - children_[child]->parent_ = thisPtr; - } - return children_[child]; + } else { + children_[child].reset(new TimingOutline(label)); + children_[child]->parent_ = thisPtr; + } + return children_[child]; } /* ************************************************************************* */ void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS - assert(timer_.is_stopped()); + assert(timer_.is_stopped()); timer_.start(); #else - assert(!timerActive_); + assert(!timerActive_); timer_.restart(); *timerActive_ = true; #endif @@ -165,9 +165,9 @@ void TimingOutline::tic() { /* ************************************************************************* */ void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS - assert(!timer_.is_stopped()); + assert(!timer_.is_stopped()); timer_.stop(); - add((timer_.elapsed().user + timer_.elapsed().system) / 1000); + add((timer_.elapsed().user + timer_.elapsed().system) / 1000); #else assert(timerActive_); double elapsed = timer_.elapsed(); @@ -178,14 +178,14 @@ void TimingOutline::toc() { /* ************************************************************************* */ void TimingOutline::finishedIteration() { - if(tIt_ > tMax_) - tMax_ = tIt_; - if(tMin_ == 0 || tIt_ < tMin_) - tMin_ = tIt_; - tIt_ = 0; - for(size_t i=0; ifinishedIteration(); + if(tIt_ > tMax_) + tMax_ = tIt_; + if(tMin_ == 0 || tIt_ < tMin_) + tMin_ = tIt_; + tIt_ = 0; + for(size_t i=0; ifinishedIteration(); } /* ************************************************************************* */ @@ -199,47 +199,47 @@ void tic_(size_t id, const std::string& label) { /* ************************************************************************* */ void toc_(size_t id) { - if(ISDEBUG("timing-verbose")) - std::cout << "toc(" << id << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) { - if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - != current->parent_.lock()->children_.end()) - std::cout << "gtsam timing: Incorrect ID passed to toc, expected " - << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() - << " \"" << current->label_ << "\", got " << id << std::endl; - else - std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; - timingRoot->print(); - throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc"); - } - current->toc(); - if(!current->parent_.lock()) { - std::cout << "gtsam timing: extra toc, already at the root" << std::endl; - timingRoot->print(); - throw std::invalid_argument("gtsam timing: extra toc, already at the root"); - } - timingCurrent = current->parent_; + if(ISDEBUG("timing-verbose")) + std::cout << "toc(" << id << ")" << std::endl; + boost::shared_ptr current(timingCurrent.lock()); + if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) { + if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) + != current->parent_.lock()->children_.end()) + std::cout << "gtsam timing: Incorrect ID passed to toc, expected " + << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() + << " \"" << current->label_ << "\", got " << id << std::endl; + else + std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; + timingRoot->print(); + throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc"); + } + current->toc(); + if(!current->parent_.lock()) { + std::cout << "gtsam timing: extra toc, already at the root" << std::endl; + timingRoot->print(); + throw std::invalid_argument("gtsam timing: extra toc, already at the root"); + } + timingCurrent = current->parent_; } /* ************************************************************************* */ void toc_(size_t id, const std::string& label) { - if(ISDEBUG("timing-verbose")) - std::cout << "toc(" << id << ", " << label << ")" << std::endl; - bool check = false; + if(ISDEBUG("timing-verbose")) + std::cout << "toc(" << id << ", " << label << ")" << std::endl; + bool check = false; #ifndef NDEBUG - // If NDEBUG is defined, still do this debug check if the granular debugging - // flag is enabled. If NDEBUG is not defined, always do this check. - check = true; + // If NDEBUG is defined, still do this debug check if the granular debugging + // flag is enabled. If NDEBUG is not defined, always do this check. + check = true; #endif - if(check || ISDEBUG("timing-debug")) { - if(label != timingCurrent.lock()->label_) { - std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl; - timingRoot->print(); - exit(1); - } - } - toc_(id); + if(check || ISDEBUG("timing-debug")) { + if(label != timingCurrent.lock()->label_) { + std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl; + timingRoot->print(); + exit(1); + } + } + toc_(id); } #ifdef ENABLE_OLD_TIMING @@ -247,12 +247,12 @@ void toc_(size_t id, const std::string& label) { /* ************************************************************************* */ // Timing class implementation void Timing::print() { - std::map::iterator it; - for(it = this->stats.begin(); it!=stats.end(); it++) { - Stats& s = it->second; - printf("%s: %g (%i times, min: %g, max: %g)\n", - it->first.c_str(), s.t, s.n, s.t_min, s.t_max); - } + std::map::iterator it; + for(it = this->stats.begin(); it!=stats.end(); it++) { + Stats& s = it->second; + printf("%s: %g (%i times, min: %g, max: %g)\n", + it->first.c_str(), s.t, s.n, s.t_min, s.t_max); + } } /* ************************************************************************* */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a42c2acc5..6d4cb192f 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -26,8 +26,8 @@ namespace gtsam { - /// Integer variable index type - typedef size_t Index; + /// Integer variable index type + typedef size_t Index; /** A function to convert indices to strings, for example by translating back * to a nonlinear key and then to a Symbol. */ @@ -38,48 +38,48 @@ namespace gtsam { /** The default IndexFormatter outputs the index */ static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; - /** - * Helper class that uses templates to select between two types based on - * whether TEST_TYPE is const or not. - */ - template - struct const_selector { - }; + /** + * Helper class that uses templates to select between two types based on + * whether TEST_TYPE is const or not. + */ + template + struct const_selector { + }; - /** Specialization for the non-const version */ - template - struct const_selector { - typedef AS_NON_CONST type; - }; + /** Specialization for the non-const version */ + template + struct const_selector { + typedef AS_NON_CONST type; + }; - /** Specialization for the const version */ - template - struct const_selector { - typedef AS_CONST type; - }; + /** Specialization for the const version */ + template + struct const_selector { + typedef AS_CONST type; + }; - /** - * Helper struct that encapsulates a value with a default, this is just used - * as a member object so you don't have to specify defaults in the class - * constructor. - */ - template - struct ValueWithDefault { - T value; + /** + * Helper struct that encapsulates a value with a default, this is just used + * as a member object so you don't have to specify defaults in the class + * constructor. + */ + template + struct ValueWithDefault { + T value; - /** Default constructor, initialize to default value supplied in template argument */ - ValueWithDefault() : value(defaultValue) {} + /** Default constructor, initialize to default value supplied in template argument */ + ValueWithDefault() : value(defaultValue) {} - /** Initialize to the given value */ - ValueWithDefault(const T& _value) : value(_value) {} + /** Initialize to the given value */ + ValueWithDefault(const T& _value) : value(_value) {} - /** Operator to access the value */ - T& operator*() { return value; } + /** Operator to access the value */ + T& operator*() { return value; } - /** Implicit conversion allows use in if statements for bool type, etc. */ - operator T() const { return value; } - }; + /** Implicit conversion allows use in if statements for bool type, etc. */ + operator T() const { return value; } + }; } diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 8afb2d617..041d4c206 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file AlgebraicDecisionTree.h - * @brief Algebraic Decision Trees - * @author Frank Dellaert - * @date Mar 14, 2011 + * @file AlgebraicDecisionTree.h + * @brief Algebraic Decision Trees + * @author Frank Dellaert + * @date Mar 14, 2011 */ #pragma once @@ -22,123 +22,123 @@ namespace gtsam { - /** - * Algebraic Decision Trees fix the range to double - * Just has some nice constructors and some syntactic sugar - * TODO: consider eliminating this class altogether? - */ - template - class AlgebraicDecisionTree: public DecisionTree { + /** + * Algebraic Decision Trees fix the range to double + * Just has some nice constructors and some syntactic sugar + * TODO: consider eliminating this class altogether? + */ + template + class AlgebraicDecisionTree: public DecisionTree { - public: + public: - typedef DecisionTree Super; + typedef DecisionTree Super; - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { - return 0.0; - } - static inline double one() { - return 1.0; - } - static inline double add(const double& a, const double& b) { - return a + b; - } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { - return a * b; - } - static inline double div(const double& a, const double& b) { - return a / b; - } - static inline double id(const double& x) { - return x; - } - }; + /** The Real ring with addition and multiplication */ + struct Ring { + static inline double zero() { + return 0.0; + } + static inline double one() { + return 1.0; + } + static inline double add(const double& a, const double& b) { + return a + b; + } + static inline double max(const double& a, const double& b) { + return std::max(a, b); + } + static inline double mul(const double& a, const double& b) { + return a * b; + } + static inline double div(const double& a, const double& b) { + return a / b; + } + static inline double id(const double& x) { + return x; + } + }; - AlgebraicDecisionTree() : - Super(1.0) { - } + AlgebraicDecisionTree() : + Super(1.0) { + } - AlgebraicDecisionTree(const Super& add) : - Super(add) { - } + AlgebraicDecisionTree(const Super& add) : + Super(add) { + } - /** Create a new leaf function splitting on a variable */ - AlgebraicDecisionTree(const L& label, double y1, double y2) : - Super(label, y1, y2) { - } + /** Create a new leaf function splitting on a variable */ + AlgebraicDecisionTree(const L& label, double y1, double y2) : + Super(label, y1, y2) { + } - /** Create a new leaf function splitting on a variable */ - AlgebraicDecisionTree(const typename Super::LabelC& labelC, double y1, double y2) : - Super(labelC, y1, y2) { - } + /** Create a new leaf function splitting on a variable */ + AlgebraicDecisionTree(const typename Super::LabelC& labelC, double y1, double y2) : + Super(labelC, y1, y2) { + } - /** Create from keys and vector table */ - AlgebraicDecisionTree // - (const std::vector& labelCs, const std::vector& ys) { - this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), - ys.end()); - } + /** Create from keys and vector table */ + AlgebraicDecisionTree // + (const std::vector& labelCs, const std::vector& ys) { + this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), + ys.end()); + } - /** Create from keys and string table */ - AlgebraicDecisionTree // - (const std::vector& labelCs, const std::string& table) { - // Convert string to doubles - std::vector ys; - std::istringstream iss(table); - std::copy(std::istream_iterator(iss), - std::istream_iterator(), std::back_inserter(ys)); + /** Create from keys and string table */ + AlgebraicDecisionTree // + (const std::vector& labelCs, const std::string& table) { + // Convert string to doubles + std::vector ys; + std::istringstream iss(table); + std::copy(std::istream_iterator(iss), + std::istream_iterator(), std::back_inserter(ys)); - // now call recursive Create - this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), - ys.end()); - } + // now call recursive Create + this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(), + ys.end()); + } - /** Create a new function splitting on a variable */ - template - AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : - Super(NULL) { - this->root_ = compose(begin, end, label); - } + /** Create a new function splitting on a variable */ + template + AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : + Super(NULL) { + this->root_ = compose(begin, end, label); + } - /** Convert */ - template - AlgebraicDecisionTree(const AlgebraicDecisionTree& other, - const std::map& map) { - this->root_ = this->template convert(other.root_, map, - Ring::id); - } + /** Convert */ + template + AlgebraicDecisionTree(const AlgebraicDecisionTree& other, + const std::map& map) { + this->root_ = this->template convert(other.root_, map, + Ring::id); + } - /** sum */ - AlgebraicDecisionTree operator+(const AlgebraicDecisionTree& g) const { - return this->apply(g, &Ring::add); - } + /** sum */ + AlgebraicDecisionTree operator+(const AlgebraicDecisionTree& g) const { + return this->apply(g, &Ring::add); + } - /** product */ - AlgebraicDecisionTree operator*(const AlgebraicDecisionTree& g) const { - return this->apply(g, &Ring::mul); - } + /** product */ + AlgebraicDecisionTree operator*(const AlgebraicDecisionTree& g) const { + return this->apply(g, &Ring::mul); + } - /** division */ - AlgebraicDecisionTree operator/(const AlgebraicDecisionTree& g) const { - return this->apply(g, &Ring::div); - } + /** division */ + AlgebraicDecisionTree operator/(const AlgebraicDecisionTree& g) const { + return this->apply(g, &Ring::div); + } - /** sum out variable */ - AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { - return this->combine(label, cardinality, &Ring::add); - } + /** sum out variable */ + AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { + return this->combine(label, cardinality, &Ring::add); + } - /** sum out variable */ - AlgebraicDecisionTree sum(const typename Super::LabelC& labelC) const { - return this->combine(labelC, &Ring::add); - } + /** sum out variable */ + AlgebraicDecisionTree sum(const typename Super::LabelC& labelC) const { + return this->combine(labelC, &Ring::add); + } - }; + }; // AlgebraicDecisionTree } diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index f6decc359..709c7350e 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file Assignment.h - * @brief An assignment from labels to a discrete value index (size_t) - * @author Frank Dellaert - * @date Feb 5, 2012 + * @file Assignment.h + * @brief An assignment from labels to a discrete value index (size_t) + * @author Frank Dellaert + * @date Feb 5, 2012 */ #pragma once @@ -26,62 +26,62 @@ namespace gtsam { - /** - * An assignment from labels to value index (size_t). - * Assigns to each label a value. Implemented as a simple map. - * A discrete factor takes an Assignment and returns a value. - */ - template - class Assignment: public std::map { - public: - void print(const std::string& s = "Assignment: ") const { - std::cout << s << ": "; - BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this) - std::cout << "(" << keyValue.first << ", " << keyValue.second << ")"; - std::cout << std::endl; - } + /** + * An assignment from labels to value index (size_t). + * Assigns to each label a value. Implemented as a simple map. + * A discrete factor takes an Assignment and returns a value. + */ + template + class Assignment: public std::map { + public: + void print(const std::string& s = "Assignment: ") const { + std::cout << s << ": "; + BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this) + std::cout << "(" << keyValue.first << ", " << keyValue.second << ")"; + std::cout << std::endl; + } - bool equals(const Assignment& other, double tol = 1e-9) const { - return (*this == other); - } - }; //Assignment + bool equals(const Assignment& other, double tol = 1e-9) const { + return (*this == other); + } + }; //Assignment - /** - * @brief Get Cartesian product consisting all possible configurations - * @param vector list of keys (label,cardinality) pairs. - * @return vector list of all possible value assignments - * - * This function returns a vector of Assignment values for all possible - * (Cartesian product) configurations of set of Keys which are nothing - * but (Label,cardinality) pairs. This function should NOT be called for - * more than a small number of variables and cardinalities. E.g. For 6 - * variables with each having cardinalities 4, we get 4096 possible - * configurations!! - */ - template - std::vector > cartesianProduct( - const std::vector >& keys) { - std::vector > allPossValues; - Assignment values; - typedef std::pair DiscreteKey; - BOOST_FOREACH(const DiscreteKey& key, keys) - values[key.first] = 0; //Initialize from 0 - while (1) { - allPossValues.push_back(values); - size_t j = 0; - for (j = 0; j < keys.size(); j++) { - L idx = keys[j].first; - values[idx]++; - if (values[idx] < keys[j].second) - break; - //Wrap condition - values[idx] = 0; - } - if (j == keys.size()) - break; - } - return allPossValues; - } + /** + * @brief Get Cartesian product consisting all possible configurations + * @param vector list of keys (label,cardinality) pairs. + * @return vector list of all possible value assignments + * + * This function returns a vector of Assignment values for all possible + * (Cartesian product) configurations of set of Keys which are nothing + * but (Label,cardinality) pairs. This function should NOT be called for + * more than a small number of variables and cardinalities. E.g. For 6 + * variables with each having cardinalities 4, we get 4096 possible + * configurations!! + */ + template + std::vector > cartesianProduct( + const std::vector >& keys) { + std::vector > allPossValues; + Assignment values; + typedef std::pair DiscreteKey; + BOOST_FOREACH(const DiscreteKey& key, keys) + values[key.first] = 0; //Initialize from 0 + while (1) { + allPossValues.push_back(values); + size_t j = 0; + for (j = 0; j < keys.size(); j++) { + L idx = keys[j].first; + values[idx]++; + if (values[idx] < keys[j].second) + break; + //Wrap condition + values[idx] = 0; + } + if (j == keys.size()) + break; + } + return allPossValues; + } } // namespace gtsam diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 057f014bf..a13dba513 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file DecisionTree.h - * @brief Decision Tree for use in DiscreteFactors - * @author Frank Dellaert - * @author Can Erdogan - * @date Jan 30, 2012 + * @file DecisionTree.h + * @brief Decision Tree for use in DiscreteFactors + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 */ #pragma once @@ -38,637 +38,637 @@ using boost::assign::operator+=; namespace gtsam { - /*********************************************************************************/ - // Node - /*********************************************************************************/ + /*********************************************************************************/ + // Node + /*********************************************************************************/ #ifdef DT_DEBUG_MEMORY - template - int DecisionTree::Node::nrNodes = 0; + template + int DecisionTree::Node::nrNodes = 0; #endif - /*********************************************************************************/ - // Leaf - /*********************************************************************************/ - template - class DecisionTree::Leaf: public DecisionTree::Node { + /*********************************************************************************/ + // Leaf + /*********************************************************************************/ + template + class DecisionTree::Leaf: public DecisionTree::Node { - /** constant stored in this leaf */ - Y constant_; + /** constant stored in this leaf */ + Y constant_; - public: + public: - /** Constructor from constant */ - Leaf(const Y& constant) : - constant_(constant) {} + /** Constructor from constant */ + Leaf(const Y& constant) : + constant_(constant) {} - /** return the constant */ - const Y& constant() const { - return constant_; - } + /** return the constant */ + const Y& constant() const { + return constant_; + } - /// Leaf-Leaf equality - bool sameLeaf(const Leaf& q) const { - return constant_ == q.constant_; - } + /// Leaf-Leaf equality + bool sameLeaf(const Leaf& q) const { + return constant_ == q.constant_; + } - /// polymorphic equality: is q is a leaf, could be - bool sameLeaf(const Node& q) const { - return (q.isLeaf() && q.sameLeaf(*this)); - } + /// polymorphic equality: is q is a leaf, could be + bool sameLeaf(const Node& q) const { + return (q.isLeaf() && q.sameLeaf(*this)); + } - /** equality up to tolerance */ - bool equals(const Node& q, double tol) const { - const Leaf* other = dynamic_cast (&q); - if (!other) return false; - return fabs(double(this->constant_ - other->constant_)) < tol; - } + /** equality up to tolerance */ + bool equals(const Node& q, double tol) const { + const Leaf* other = dynamic_cast (&q); + if (!other) return false; + return fabs(double(this->constant_ - other->constant_)) < tol; + } - /** print */ - void print(const std::string& s) const { - bool showZero = true; - if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; - } + /** print */ + void print(const std::string& s) const { + bool showZero = true; + if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl; + } - /** to graphviz file */ - void dot(std::ostream& os, bool showZero) const { - if (showZero || constant_) os << "\"" << this->id() << "\" [label=\"" - << boost::format("%4.2g") % constant_ - << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, - } + /** to graphviz file */ + void dot(std::ostream& os, bool showZero) const { + if (showZero || constant_) os << "\"" << this->id() << "\" [label=\"" + << boost::format("%4.2g") % constant_ + << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55, + } - /** evaluate */ - const Y& operator()(const Assignment& x) const { - return constant_; - } + /** evaluate */ + const Y& operator()(const Assignment& x) const { + return constant_; + } - /** apply unary operator */ - NodePtr apply(const Unary& op) const { - NodePtr f(new Leaf(op(constant_))); - return f; - } + /** apply unary operator */ + NodePtr apply(const Unary& op) const { + NodePtr f(new Leaf(op(constant_))); + return f; + } - // Apply binary operator "h = f op g" on Leaf node - // Note op is not assumed commutative so we need to keep track of order - // Simply calls apply on argument to call correct virtual method: - // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below) - // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) - NodePtr apply_f_op_g(const Node& g, const Binary& op) const { - return g.apply_g_op_fL(*this, op); - } + // Apply binary operator "h = f op g" on Leaf node + // Note op is not assumed commutative so we need to keep track of order + // Simply calls apply on argument to call correct virtual method: + // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below) + // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) + NodePtr apply_f_op_g(const Node& g, const Binary& op) const { + return g.apply_g_op_fL(*this, op); + } - // Applying binary operator to two leaves results in a leaf - NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { - NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL - return h; - } + // Applying binary operator to two leaves results in a leaf + NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { + NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL + return h; + } - // If second argument is a Choice node, call it's apply with leaf as second - NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { - return fC.apply_fC_op_gL(*this, op); // operand order back to normal - } + // If second argument is a Choice node, call it's apply with leaf as second + NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { + return fC.apply_fC_op_gL(*this, op); // operand order back to normal + } - /** choose a branch, create new memory ! */ - NodePtr choose(const L& label, size_t index) const { - return NodePtr(new Leaf(constant())); - } + /** choose a branch, create new memory ! */ + NodePtr choose(const L& label, size_t index) const { + return NodePtr(new Leaf(constant())); + } - bool isLeaf() const { return true; } + bool isLeaf() const { return true; } - }; // Leaf + }; // Leaf - /*********************************************************************************/ - // Choice - /*********************************************************************************/ - template - class DecisionTree::Choice: public DecisionTree::Node { + /*********************************************************************************/ + // Choice + /*********************************************************************************/ + template + class DecisionTree::Choice: public DecisionTree::Node { - /** the label of the variable on which we split */ - L label_; + /** the label of the variable on which we split */ + L label_; - /** The children of this Choice node. */ - std::vector branches_; + /** The children of this Choice node. */ + std::vector branches_; - private: - /** incremental allSame */ - size_t allSame_; + private: + /** incremental allSame */ + size_t allSame_; - typedef boost::shared_ptr ChoicePtr; + typedef boost::shared_ptr ChoicePtr; - public: + public: - virtual ~Choice() { + virtual ~Choice() { #ifdef DT_DEBUG_MEMORY - std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() << std::std::endl; + std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() << std::std::endl; #endif - } + } - /** If all branches of a choice node f are the same, just return a branch */ - static NodePtr Unique(const ChoicePtr& f) { + /** If all branches of a choice node f are the same, just return a branch */ + static NodePtr Unique(const ChoicePtr& f) { #ifndef DT_NO_PRUNING - if (f->allSame_) { - assert(f->branches().size() > 0); - NodePtr f0 = f->branches_[0]; - assert(f0->isLeaf()); - NodePtr newLeaf(new Leaf(boost::dynamic_pointer_cast(f0)->constant())); - return newLeaf; - } else + if (f->allSame_) { + assert(f->branches().size() > 0); + NodePtr f0 = f->branches_[0]; + assert(f0->isLeaf()); + NodePtr newLeaf(new Leaf(boost::dynamic_pointer_cast(f0)->constant())); + return newLeaf; + } else #endif - return f; - } + return f; + } - bool isLeaf() const { return false; } + bool isLeaf() const { return false; } - /** Constructor, given choice label and mandatory expected branch count */ - Choice(const L& label, size_t count) : - label_(label), allSame_(true) { - branches_.reserve(count); - } + /** Constructor, given choice label and mandatory expected branch count */ + Choice(const L& label, size_t count) : + label_(label), allSame_(true) { + branches_.reserve(count); + } - /** - * Construct from applying binary op to two Choice nodes - */ - Choice(const Choice& f, const Choice& g, const Binary& op) : - allSame_(true) { + /** + * Construct from applying binary op to two Choice nodes + */ + Choice(const Choice& f, const Choice& g, const Binary& op) : + allSame_(true) { - // Choose what to do based on label - if (f.label() > g.label()) { - // f higher than g - label_ = f.label(); - size_t count = f.nrChoices(); - branches_.reserve(count); - for (size_t i = 0; i < count; i++) - push_back(f.branches_[i]->apply_f_op_g(g, op)); - } else if (g.label() > f.label()) { - // f lower than g - label_ = g.label(); - size_t count = g.nrChoices(); - branches_.reserve(count); - for (size_t i = 0; i < count; i++) - push_back(g.branches_[i]->apply_g_op_fC(f, op)); - } else { - // f same level as g - label_ = f.label(); - size_t count = f.nrChoices(); - branches_.reserve(count); - for (size_t i = 0; i < count; i++) - push_back(f.branches_[i]->apply_f_op_g(*g.branches_[i], op)); - } - } + // Choose what to do based on label + if (f.label() > g.label()) { + // f higher than g + label_ = f.label(); + size_t count = f.nrChoices(); + branches_.reserve(count); + for (size_t i = 0; i < count; i++) + push_back(f.branches_[i]->apply_f_op_g(g, op)); + } else if (g.label() > f.label()) { + // f lower than g + label_ = g.label(); + size_t count = g.nrChoices(); + branches_.reserve(count); + for (size_t i = 0; i < count; i++) + push_back(g.branches_[i]->apply_g_op_fC(f, op)); + } else { + // f same level as g + label_ = f.label(); + size_t count = f.nrChoices(); + branches_.reserve(count); + for (size_t i = 0; i < count; i++) + push_back(f.branches_[i]->apply_f_op_g(*g.branches_[i], op)); + } + } - const L& label() const { - return label_; - } + const L& label() const { + return label_; + } - size_t nrChoices() const { - return branches_.size(); - } + size_t nrChoices() const { + return branches_.size(); + } - const std::vector& branches() const { - return branches_; - } + const std::vector& branches() const { + return branches_; + } - /** add a branch: TODO merge into constructor */ - void push_back(const NodePtr& node) { - // allSame_ is restricted to leaf nodes in a decision tree - if (allSame_ && !branches_.empty()) { - allSame_ = node->sameLeaf(*branches_.back()); - } - branches_.push_back(node); - } + /** add a branch: TODO merge into constructor */ + void push_back(const NodePtr& node) { + // allSame_ is restricted to leaf nodes in a decision tree + if (allSame_ && !branches_.empty()) { + allSame_ = node->sameLeaf(*branches_.back()); + } + branches_.push_back(node); + } - /** print (as a tree) */ - void print(const std::string& s) const { - std::cout << s << " Choice("; - // std::cout << this << ","; - std::cout << label_ << ") " << std::endl; + /** print (as a tree) */ + void print(const std::string& s) const { + std::cout << s << " Choice("; + // std::cout << this << ","; + std::cout << label_ << ") " << std::endl; for (size_t i = 0; i < branches_.size(); i++) - branches_[i]->print((boost::format("%s %d") % s % i).str()); - } + branches_[i]->print((boost::format("%s %d") % s % i).str()); + } - /** output to graphviz (as a a graph) */ - void dot(std::ostream& os, bool showZero) const { - os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ - << "\"]\n"; - for (size_t i = 0; i < branches_.size(); i++) { - NodePtr branch = branches_[i]; + /** output to graphviz (as a a graph) */ + void dot(std::ostream& os, bool showZero) const { + os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ + << "\"]\n"; + for (size_t i = 0; i < branches_.size(); i++) { + NodePtr branch = branches_[i]; - // Check if zero - if (!showZero) { - const Leaf* leaf = dynamic_cast (branch.get()); - if (leaf && !leaf->constant()) continue; - } + // Check if zero + if (!showZero) { + const Leaf* leaf = dynamic_cast (branch.get()); + if (leaf && !leaf->constant()) continue; + } - os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; - if (i == 0) os << " [style=dashed]"; - if (i > 1) os << " [style=bold]"; - os << std::endl; - branch->dot(os, showZero); - } - } + os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; + if (i == 0) os << " [style=dashed]"; + if (i > 1) os << " [style=bold]"; + os << std::endl; + branch->dot(os, showZero); + } + } - /// Choice-Leaf equality: always false - bool sameLeaf(const Leaf& q) const { - return false; - } + /// Choice-Leaf equality: always false + bool sameLeaf(const Leaf& q) const { + return false; + } - /// polymorphic equality: if q is a leaf, could be... - bool sameLeaf(const Node& q) const { - return (q.isLeaf() && q.sameLeaf(*this)); - } + /// polymorphic equality: if q is a leaf, could be... + bool sameLeaf(const Node& q) const { + return (q.isLeaf() && q.sameLeaf(*this)); + } - /** equality up to tolerance */ - bool equals(const Node& q, double tol) const { - const Choice* other = dynamic_cast (&q); - if (!other) return false; - if (this->label_ != other->label_) return false; - if (branches_.size() != other->branches_.size()) return false; - // we don't care about shared pointers being equal here - for (size_t i = 0; i < branches_.size(); i++) - if (!(branches_[i]->equals(*(other->branches_[i]), tol))) return false; - return true; - } + /** equality up to tolerance */ + bool equals(const Node& q, double tol) const { + const Choice* other = dynamic_cast (&q); + if (!other) return false; + if (this->label_ != other->label_) return false; + if (branches_.size() != other->branches_.size()) return false; + // we don't care about shared pointers being equal here + for (size_t i = 0; i < branches_.size(); i++) + if (!(branches_[i]->equals(*(other->branches_[i]), tol))) return false; + return true; + } - /** evaluate */ - const Y& operator()(const Assignment& x) const { + /** evaluate */ + const Y& operator()(const Assignment& x) const { #ifndef NDEBUG - typename Assignment::const_iterator it = x.find(label_); - if (it == x.end()) { - std::cout << "Trying to find value for " << label_ << std::endl; - throw std::invalid_argument( - "DecisionTree::operator(): value undefined for a label"); - } + typename Assignment::const_iterator it = x.find(label_); + if (it == x.end()) { + std::cout << "Trying to find value for " << label_ << std::endl; + throw std::invalid_argument( + "DecisionTree::operator(): value undefined for a label"); + } #endif - size_t index = x.at(label_); - NodePtr child = branches_[index]; - return (*child)(x); - } + size_t index = x.at(label_); + NodePtr child = branches_[index]; + return (*child)(x); + } - /** - * Construct from applying unary op to a Choice node - */ - Choice(const L& label, const Choice& f, const Unary& op) : - label_(label), allSame_(true) { + /** + * Construct from applying unary op to a Choice node + */ + Choice(const L& label, const Choice& f, const Unary& op) : + label_(label), allSame_(true) { - branches_.reserve(f.branches_.size()); // reserve space - BOOST_FOREACH (const NodePtr& branch, f.branches_) - push_back(branch->apply(op)); - } + branches_.reserve(f.branches_.size()); // reserve space + BOOST_FOREACH (const NodePtr& branch, f.branches_) + push_back(branch->apply(op)); + } - /** apply unary operator */ - NodePtr apply(const Unary& op) const { - boost::shared_ptr r(new Choice(label_, *this, op)); - return Unique(r); - } + /** apply unary operator */ + NodePtr apply(const Unary& op) const { + boost::shared_ptr r(new Choice(label_, *this, op)); + return Unique(r); + } - // Apply binary operator "h = f op g" on Choice node - // Note op is not assumed commutative so we need to keep track of order - // Simply calls apply on argument to call correct virtual method: - // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf) - // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) - NodePtr apply_f_op_g(const Node& g, const Binary& op) const { - return g.apply_g_op_fC(*this, op); - } + // Apply binary operator "h = f op g" on Choice node + // Note op is not assumed commutative so we need to keep track of order + // Simply calls apply on argument to call correct virtual method: + // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf) + // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) + NodePtr apply_f_op_g(const Node& g, const Binary& op) const { + return g.apply_g_op_fC(*this, op); + } - // If second argument of binary op is Leaf node, recurse on branches - NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { - boost::shared_ptr h(new Choice(label(), nrChoices())); - BOOST_FOREACH(NodePtr branch, branches_) - h->push_back(fL.apply_f_op_g(*branch, op)); - return Unique(h); - } + // If second argument of binary op is Leaf node, recurse on branches + NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const { + boost::shared_ptr h(new Choice(label(), nrChoices())); + BOOST_FOREACH(NodePtr branch, branches_) + h->push_back(fL.apply_f_op_g(*branch, op)); + return Unique(h); + } - // If second argument of binary op is Choice, call constructor - NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { - boost::shared_ptr h(new Choice(fC, *this, op)); - return Unique(h); - } + // If second argument of binary op is Choice, call constructor + NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const { + boost::shared_ptr h(new Choice(fC, *this, op)); + return Unique(h); + } - // If second argument of binary op is Leaf - template - NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const { - boost::shared_ptr h(new Choice(label(), nrChoices())); - BOOST_FOREACH(const NodePtr& branch, branches_) - h->push_back(branch->apply_f_op_g(gL, op)); - return Unique(h); - } + // If second argument of binary op is Leaf + template + NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const { + boost::shared_ptr h(new Choice(label(), nrChoices())); + BOOST_FOREACH(const NodePtr& branch, branches_) + h->push_back(branch->apply_f_op_g(gL, op)); + return Unique(h); + } - /** choose a branch, recursively */ - NodePtr choose(const L& label, size_t index) const { - if (label_ == label) - return branches_[index]; // choose branch - - // second case, not label of interest, just recurse - boost::shared_ptr r(new Choice(label_, branches_.size())); - BOOST_FOREACH(const NodePtr& branch, branches_) - r->push_back(branch->choose(label, index)); - return Unique(r); - } + /** choose a branch, recursively */ + NodePtr choose(const L& label, size_t index) const { + if (label_ == label) + return branches_[index]; // choose branch + + // second case, not label of interest, just recurse + boost::shared_ptr r(new Choice(label_, branches_.size())); + BOOST_FOREACH(const NodePtr& branch, branches_) + r->push_back(branch->choose(label, index)); + return Unique(r); + } - }; // Choice + }; // Choice - /*********************************************************************************/ - // DecisionTree - /*********************************************************************************/ - template - DecisionTree::DecisionTree() { - } + /*********************************************************************************/ + // DecisionTree + /*********************************************************************************/ + template + DecisionTree::DecisionTree() { + } - template - DecisionTree::DecisionTree(const NodePtr& root) : - root_(root) { - } + template + DecisionTree::DecisionTree(const NodePtr& root) : + root_(root) { + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const Y& y) { - root_ = NodePtr(new Leaf(y)); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const Y& y) { + root_ = NodePtr(new Leaf(y)); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(// - const L& label, const Y& y1, const Y& y2) { - boost::shared_ptr a(new Choice(label, 2)); - NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); - a->push_back(l1); - a->push_back(l2); - root_ = Choice::Unique(a); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(// + const L& label, const Y& y1, const Y& y2) { + boost::shared_ptr a(new Choice(label, 2)); + NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); + a->push_back(l1); + a->push_back(l2); + root_ = Choice::Unique(a); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(// - const LabelC& labelC, const Y& y1, const Y& y2) { - if (labelC.second != 2) throw std::invalid_argument( - "DecisionTree: binary constructor called with non-binary label"); - boost::shared_ptr a(new Choice(labelC.first, 2)); - NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); - a->push_back(l1); - a->push_back(l2); - root_ = Choice::Unique(a); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(// + const LabelC& labelC, const Y& y1, const Y& y2) { + if (labelC.second != 2) throw std::invalid_argument( + "DecisionTree: binary constructor called with non-binary label"); + boost::shared_ptr a(new Choice(labelC.first, 2)); + NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); + a->push_back(l1); + a->push_back(l2); + root_ = Choice::Unique(a); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const std::vector& labelCs, - const std::vector& ys) { - // call recursive Create - root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const std::vector& labelCs, + const std::vector& ys) { + // call recursive Create + root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const std::vector& labelCs, - const std::string& table) { + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const std::vector& labelCs, + const std::string& table) { - // Convert std::string to doubles - std::vector ys; - std::istringstream iss(table); - copy(std::istream_iterator(iss), std::istream_iterator(), - back_inserter(ys)); + // Convert std::string to doubles + std::vector ys; + std::istringstream iss(table); + copy(std::istream_iterator(iss), std::istream_iterator(), + back_inserter(ys)); - // now call recursive Create - root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); - } + // now call recursive Create + root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); + } - /*********************************************************************************/ - template - template DecisionTree::DecisionTree( - Iterator begin, Iterator end, const L& label) { - root_ = compose(begin, end, label); - } + /*********************************************************************************/ + template + template DecisionTree::DecisionTree( + Iterator begin, Iterator end, const L& label) { + root_ = compose(begin, end, label); + } - /*********************************************************************************/ - template - DecisionTree::DecisionTree(const L& label, - const DecisionTree& f0, const DecisionTree& f1) { - std::vector functions; - functions += f0, f1; - root_ = compose(functions.begin(), functions.end(), label); - } + /*********************************************************************************/ + template + DecisionTree::DecisionTree(const L& label, + const DecisionTree& f0, const DecisionTree& f1) { + std::vector functions; + functions += f0, f1; + root_ = compose(functions.begin(), functions.end(), label); + } - /*********************************************************************************/ - template - template - DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op) { - root_ = convert(other.root_, map, op); - } + /*********************************************************************************/ + template + template + DecisionTree::DecisionTree(const DecisionTree& other, + const std::map& map, boost::function op) { + root_ = convert(other.root_, map, op); + } - /*********************************************************************************/ - // Called by two constructors above. - // Takes a label and a corresponding range of decision trees, and creates a new - // decision tree. However, the order of the labels needs to be respected, so we - // cannot just create a root Choice node on the label: if the label is not the - // highest label, we need to do a complicated and expensive recursive call. - template template - typename DecisionTree::NodePtr DecisionTree::compose( - Iterator begin, Iterator end, const L& label) const { + /*********************************************************************************/ + // Called by two constructors above. + // Takes a label and a corresponding range of decision trees, and creates a new + // decision tree. However, the order of the labels needs to be respected, so we + // cannot just create a root Choice node on the label: if the label is not the + // highest label, we need to do a complicated and expensive recursive call. + template template + typename DecisionTree::NodePtr DecisionTree::compose( + Iterator begin, Iterator end, const L& label) const { - // find highest label among branches - boost::optional highestLabel; - boost::optional nrChoices; - for (Iterator it = begin; it != end; it++) { - if (it->root_->isLeaf()) continue; - boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); - if (!highestLabel || c->label() > *highestLabel) { - highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); - } - } + // find highest label among branches + boost::optional highestLabel; + boost::optional nrChoices; + for (Iterator it = begin; it != end; it++) { + if (it->root_->isLeaf()) continue; + boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); + if (!highestLabel || c->label() > *highestLabel) { + highestLabel.reset(c->label()); + nrChoices.reset(c->nrChoices()); + } + } - // if label is already in correct order, just put together a choice on label - if (!highestLabel || label > *highestLabel) { - boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); - for (Iterator it = begin; it != end; it++) - choiceOnLabel->push_back(it->root_); - return Choice::Unique(choiceOnLabel); - } + // if label is already in correct order, just put together a choice on label + if (!highestLabel || label > *highestLabel) { + boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); + for (Iterator it = begin; it != end; it++) + choiceOnLabel->push_back(it->root_); + return Choice::Unique(choiceOnLabel); + } - // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); - // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { - // make a new set of functions for composing by iterating over the given - // functions, and selecting the appropriate branch. - std::vector functions; - for (Iterator it = begin; it != end; it++) { - // by restricting the input functions to value i for labelBelow - DecisionTree chosen = it->choose(*highestLabel, index); - functions.push_back(chosen); - } - // We then recurse, for all values of the highest label - NodePtr fi = compose(functions.begin(), functions.end(), label); - choiceOnHighestLabel->push_back(fi); - } - return Choice::Unique(choiceOnHighestLabel); - } + // Set up a new choice on the highest label + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); + // now, for all possible values of highestLabel + for (size_t index = 0; index < *nrChoices; index++) { + // make a new set of functions for composing by iterating over the given + // functions, and selecting the appropriate branch. + std::vector functions; + for (Iterator it = begin; it != end; it++) { + // by restricting the input functions to value i for labelBelow + DecisionTree chosen = it->choose(*highestLabel, index); + functions.push_back(chosen); + } + // We then recurse, for all values of the highest label + NodePtr fi = compose(functions.begin(), functions.end(), label); + choiceOnHighestLabel->push_back(fi); + } + return Choice::Unique(choiceOnHighestLabel); + } - /*********************************************************************************/ - // "create" is a bit of a complicated thing, but very useful. - // It takes a range of labels and a corresponding range of values, - // and creates a decision tree, as follows: - // - if there is only one label, creates a choice node with values in leaves - // - otherwise, it evenly splits up the range of values and creates a tree for - // each sub-range, and assigns that tree to first label's choices - // Example: - // create([B A],[1 2 3 4]) would call - // create([A],[1 2]) - // create([A],[3 4]) - // and produce - // B=0 - // A=0: 1 - // A=1: 2 - // B=1 - // A=0: 3 - // A=1: 4 - // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce - // exactly the same tree as above: the highest label is always the root. - // However, it will be *way* faster if labels are given highest to lowest. - template - template - typename DecisionTree::NodePtr DecisionTree::create( - It begin, It end, ValueIt beginY, ValueIt endY) const { + /*********************************************************************************/ + // "create" is a bit of a complicated thing, but very useful. + // It takes a range of labels and a corresponding range of values, + // and creates a decision tree, as follows: + // - if there is only one label, creates a choice node with values in leaves + // - otherwise, it evenly splits up the range of values and creates a tree for + // each sub-range, and assigns that tree to first label's choices + // Example: + // create([B A],[1 2 3 4]) would call + // create([A],[1 2]) + // create([A],[3 4]) + // and produce + // B=0 + // A=0: 1 + // A=1: 2 + // B=1 + // A=0: 3 + // A=1: 4 + // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce + // exactly the same tree as above: the highest label is always the root. + // However, it will be *way* faster if labels are given highest to lowest. + template + template + typename DecisionTree::NodePtr DecisionTree::create( + It begin, It end, ValueIt beginY, ValueIt endY) const { - // get crucial counts - size_t nrChoices = begin->second; - size_t size = endY - beginY; + // get crucial counts + size_t nrChoices = begin->second; + size_t size = endY - beginY; - // Find the next key to work on - It labelC = begin + 1; - if (labelC == end) { - // Base case: only one key left - // Create a simple choice node with values as leaves. - if (size != nrChoices) { - std::cout << "Trying to create DD on " << begin->first << std::endl; - std::cout << boost::format("DecisionTree::create: expected %d values but got %d instead") % nrChoices % size << std::endl; - throw std::invalid_argument("DecisionTree::create invalid argument"); - } - boost::shared_ptr choice(new Choice(begin->first, endY - beginY)); - for (ValueIt y = beginY; y != endY; y++) - choice->push_back(NodePtr(new Leaf(*y))); - return Choice::Unique(choice); - } + // Find the next key to work on + It labelC = begin + 1; + if (labelC == end) { + // Base case: only one key left + // Create a simple choice node with values as leaves. + if (size != nrChoices) { + std::cout << "Trying to create DD on " << begin->first << std::endl; + std::cout << boost::format("DecisionTree::create: expected %d values but got %d instead") % nrChoices % size << std::endl; + throw std::invalid_argument("DecisionTree::create invalid argument"); + } + boost::shared_ptr choice(new Choice(begin->first, endY - beginY)); + for (ValueIt y = beginY; y != endY; y++) + choice->push_back(NodePtr(new Leaf(*y))); + return Choice::Unique(choice); + } - // Recursive case: perform "Shannon expansion" - // Creates one tree (i.e.,function) for each choice of current key - // by calling create recursively, and then puts them all together. - std::vector functions; - size_t split = size / nrChoices; - for (size_t i = 0; i < nrChoices; i++, beginY += split) { - NodePtr f = create(labelC, end, beginY, beginY + split); - functions += DecisionTree(f); - } - return compose(functions.begin(), functions.end(), begin->first); - } + // Recursive case: perform "Shannon expansion" + // Creates one tree (i.e.,function) for each choice of current key + // by calling create recursively, and then puts them all together. + std::vector functions; + size_t split = size / nrChoices; + for (size_t i = 0; i < nrChoices; i++, beginY += split) { + NodePtr f = create(labelC, end, beginY, beginY + split); + functions += DecisionTree(f); + } + return compose(functions.begin(), functions.end(), begin->first); + } - /*********************************************************************************/ - template - template - typename DecisionTree::NodePtr DecisionTree::convert( - const typename DecisionTree::NodePtr& f, const std::map& map, - boost::function op) { + /*********************************************************************************/ + template + template + typename DecisionTree::NodePtr DecisionTree::convert( + const typename DecisionTree::NodePtr& f, const std::map& map, + boost::function op) { - typedef DecisionTree MX; - typedef typename MX::Leaf MXLeaf; - typedef typename MX::Choice MXChoice; - typedef typename MX::NodePtr MXNodePtr; - typedef DecisionTree LY; + typedef DecisionTree MX; + typedef typename MX::Leaf MXLeaf; + typedef typename MX::Choice MXChoice; + typedef typename MX::NodePtr MXNodePtr; + typedef DecisionTree LY; - // ugliness below because apparently we can't have templated virtual functions - // If leaf, apply unary conversion "op" and create a unique leaf - const MXLeaf* leaf = dynamic_cast (f.get()); - if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); + // ugliness below because apparently we can't have templated virtual functions + // If leaf, apply unary conversion "op" and create a unique leaf + const MXLeaf* leaf = dynamic_cast (f.get()); + if (leaf) return NodePtr(new Leaf(op(leaf->constant()))); - // Check if Choice - boost::shared_ptr choice = boost::dynamic_pointer_cast (f); - if (!choice) throw std::invalid_argument( - "DecisionTree::Convert: Invalid NodePtr"); + // Check if Choice + boost::shared_ptr choice = boost::dynamic_pointer_cast (f); + if (!choice) throw std::invalid_argument( + "DecisionTree::Convert: Invalid NodePtr"); - // get new label - M oldLabel = choice->label(); - L newLabel = map.at(oldLabel); + // get new label + M oldLabel = choice->label(); + L newLabel = map.at(oldLabel); - // put together via Shannon expansion otherwise not sorted. - std::vector functions; - BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) { - LY converted(convert(branch, map, op)); - functions += converted; - } - return LY::compose(functions.begin(), functions.end(), newLabel); - } + // put together via Shannon expansion otherwise not sorted. + std::vector functions; + BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) { + LY converted(convert(branch, map, op)); + functions += converted; + } + return LY::compose(functions.begin(), functions.end(), newLabel); + } - /*********************************************************************************/ - template - bool DecisionTree::equals(const DecisionTree& other, double tol) const { - return root_->equals(*other.root_, tol); - } + /*********************************************************************************/ + template + bool DecisionTree::equals(const DecisionTree& other, double tol) const { + return root_->equals(*other.root_, tol); + } - template - void DecisionTree::print(const std::string& s) const { - root_->print(s); - } + template + void DecisionTree::print(const std::string& s) const { + root_->print(s); + } - template - bool DecisionTree::operator==(const DecisionTree& other) const { - return root_->equals(*other.root_); - } + template + bool DecisionTree::operator==(const DecisionTree& other) const { + return root_->equals(*other.root_); + } - template - const Y& DecisionTree::operator()(const Assignment& x) const { - return root_->operator ()(x); - } + template + const Y& DecisionTree::operator()(const Assignment& x) const { + return root_->operator ()(x); + } - template - DecisionTree DecisionTree::apply(const Unary& op) const { - return DecisionTree(root_->apply(op)); - } + template + DecisionTree DecisionTree::apply(const Unary& op) const { + return DecisionTree(root_->apply(op)); + } - /*********************************************************************************/ - template - DecisionTree DecisionTree::apply(const DecisionTree& g, - const Binary& op) const { - // apply the operaton on the root of both diagrams + /*********************************************************************************/ + template + DecisionTree DecisionTree::apply(const DecisionTree& g, + const Binary& op) const { + // apply the operaton on the root of both diagrams NodePtr h = root_->apply_f_op_g(*g.root_, op); // create a new class with the resulting root "h" - DecisionTree result(h); - return result; - } + DecisionTree result(h); + return result; + } - /*********************************************************************************/ - // The way this works: - // We have an ADT, picture it as a tree. - // At a certain depth, we have a branch on "label". - // The function "choose(label,index)" will return a tree of one less depth, - // where there is no more branch on "label": only the subtree under that - // branch point corresponding to the value "index" is left instead. - // The function below get all these smaller trees and "ops" them together. - template - DecisionTree DecisionTree::combine(const L& label, - size_t cardinality, const Binary& op) const { - DecisionTree result = choose(label, 0); - for (size_t index = 1; index < cardinality; index++) { - DecisionTree chosen = choose(label, index); - result = result.apply(chosen, op); - } - return result; - } + /*********************************************************************************/ + // The way this works: + // We have an ADT, picture it as a tree. + // At a certain depth, we have a branch on "label". + // The function "choose(label,index)" will return a tree of one less depth, + // where there is no more branch on "label": only the subtree under that + // branch point corresponding to the value "index" is left instead. + // The function below get all these smaller trees and "ops" them together. + template + DecisionTree DecisionTree::combine(const L& label, + size_t cardinality, const Binary& op) const { + DecisionTree result = choose(label, 0); + for (size_t index = 1; index < cardinality; index++) { + DecisionTree chosen = choose(label, index); + result = result.apply(chosen, op); + } + return result; + } - /*********************************************************************************/ - template - void DecisionTree::dot(std::ostream& os, bool showZero) const { - os << "digraph G {\n"; - root_->dot(os, showZero); - os << " [ordering=out]}" << std::endl; - } + /*********************************************************************************/ + template + void DecisionTree::dot(std::ostream& os, bool showZero) const { + os << "digraph G {\n"; + root_->dot(os, showZero); + os << " [ordering=out]}" << std::endl; + } - template - void DecisionTree::dot(const std::string& name, bool showZero) const { - std::ofstream os((name + ".dot").c_str()); - dot(os, showZero); - system( - ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); - } + template + void DecisionTree::dot(const std::string& name, bool showZero) const { + std::ofstream os((name + ".dot").c_str()); + dot(os, showZero); + system( + ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); + } /*********************************************************************************/ diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index 8fabd6e82..d579ef5de 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file DecisionTree.h - * @brief Decision Tree for use in DiscreteFactors - * @author Frank Dellaert - * @author Can Erdogan - * @date Jan 30, 2012 + * @file DecisionTree.h + * @brief Decision Tree for use in DiscreteFactors + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 */ #pragma once @@ -27,203 +27,203 @@ namespace gtsam { - /** - * Algebraic Decision Trees - * L = label for variables - * Y = function range (any algebra), e.g., bool, int, double - */ - template - class DecisionTree { + /** + * Algebraic Decision Trees + * L = label for variables + * Y = function range (any algebra), e.g., bool, int, double + */ + template + class DecisionTree { - public: + public: - /** Handy typedefs for unary and binary function types */ - typedef boost::function Unary; - typedef boost::function Binary; + /** Handy typedefs for unary and binary function types */ + typedef boost::function Unary; + typedef boost::function Binary; - /** A label annotated with cardinality */ - typedef std::pair LabelC; + /** A label annotated with cardinality */ + typedef std::pair LabelC; - /** DD's consist of Leaf and Choice nodes, both subclasses of Node */ - class Leaf; - class Choice; + /** DD's consist of Leaf and Choice nodes, both subclasses of Node */ + class Leaf; + class Choice; - /** ------------------------ Node base class --------------------------- */ - class Node { - public: - typedef boost::shared_ptr Ptr; + /** ------------------------ Node base class --------------------------- */ + class Node { + public: + typedef boost::shared_ptr Ptr; #ifdef DT_DEBUG_MEMORY - static int nrNodes; + static int nrNodes; #endif - // Constructor - Node() { + // Constructor + Node() { #ifdef DT_DEBUG_MEMORY - std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); + std::cout << ++nrNodes << " constructed " << id() << std::endl; std::cout.flush(); #endif - } + } - // Destructor - virtual ~Node() { + // Destructor + virtual ~Node() { #ifdef DT_DEBUG_MEMORY - std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); + std::cout << --nrNodes << " destructed " << id() << std::endl; std::cout.flush(); #endif - } + } - // Unique ID for dot files - const void* id() const { return this; } + // Unique ID for dot files + const void* id() const { return this; } - // everything else is virtual, no documentation here as internal - virtual void print(const std::string& s = "") const = 0; - virtual void dot(std::ostream& os, bool showZero) const = 0; - virtual bool sameLeaf(const Leaf& q) const = 0; - virtual bool sameLeaf(const Node& q) const = 0; - virtual bool equals(const Node& other, double tol = 1e-9) const = 0; - virtual const Y& operator()(const Assignment& x) const = 0; - virtual Ptr apply(const Unary& op) const = 0; - virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; - virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0; - virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0; - virtual Ptr choose(const L& label, size_t index) const = 0; - virtual bool isLeaf() const = 0; - }; - /** ------------------------ Node base class --------------------------- */ + // everything else is virtual, no documentation here as internal + virtual void print(const std::string& s = "") const = 0; + virtual void dot(std::ostream& os, bool showZero) const = 0; + virtual bool sameLeaf(const Leaf& q) const = 0; + virtual bool sameLeaf(const Node& q) const = 0; + virtual bool equals(const Node& other, double tol = 1e-9) const = 0; + virtual const Y& operator()(const Assignment& x) const = 0; + virtual Ptr apply(const Unary& op) const = 0; + virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0; + virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0; + virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0; + virtual Ptr choose(const L& label, size_t index) const = 0; + virtual bool isLeaf() const = 0; + }; + /** ------------------------ Node base class --------------------------- */ - public: + public: - /** A function is a shared pointer to the root of an ADD */ - typedef typename Node::Ptr NodePtr; + /** A function is a shared pointer to the root of an ADD */ + typedef typename Node::Ptr NodePtr; - /* an AlgebraicDecisionTree just contains the root */ - NodePtr root_; + /* an AlgebraicDecisionTree just contains the root */ + NodePtr root_; - protected: + protected: - /** Internal recursive function to create from keys, cardinalities, and Y values */ - template - NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; + /** Internal recursive function to create from keys, cardinalities, and Y values */ + template + NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const; - /** Convert to a different type */ - template NodePtr - convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); + /** Convert to a different type */ + template NodePtr + convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); - /** Default constructor */ - DecisionTree(); + /** Default constructor */ + DecisionTree(); - public: + public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** Create a constant */ - DecisionTree(const Y& y); + /** Create a constant */ + DecisionTree(const Y& y); - /** Create a new leaf function splitting on a variable */ - DecisionTree(const L& label, const Y& y1, const Y& y2); + /** Create a new leaf function splitting on a variable */ + DecisionTree(const L& label, const Y& y1, const Y& y2); - /** Allow Label+Cardinality for convenience */ - DecisionTree(const LabelC& label, const Y& y1, const Y& y2); + /** Allow Label+Cardinality for convenience */ + DecisionTree(const LabelC& label, const Y& y1, const Y& y2); - /** Create from keys and string table */ - DecisionTree(const std::vector& labelCs, const std::vector& ys); + /** Create from keys and string table */ + DecisionTree(const std::vector& labelCs, const std::vector& ys); - /** Create from keys and string table */ - DecisionTree(const std::vector& labelCs, const std::string& table); + /** Create from keys and string table */ + DecisionTree(const std::vector& labelCs, const std::string& table); - /** Create DecisionTree from others */ - template - DecisionTree(Iterator begin, Iterator end, const L& label); + /** Create DecisionTree from others */ + template + DecisionTree(Iterator begin, Iterator end, const L& label); - /** Create DecisionTree from others others (binary version) */ - DecisionTree(const L& label, // - const DecisionTree& f0, const DecisionTree& f1); + /** Create DecisionTree from others others (binary version) */ + DecisionTree(const L& label, // + const DecisionTree& f0, const DecisionTree& f1); - /** Convert from a different type */ - template - DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op); + /** Convert from a different type */ + template + DecisionTree(const DecisionTree& other, + const std::map& map, boost::function op); - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ - /** GTSAM-style print */ - void print(const std::string& s = "DecisionTree") const; + /** GTSAM-style print */ + void print(const std::string& s = "DecisionTree") const; - // Testable - bool equals(const DecisionTree& other, double tol = 1e-9) const; + // Testable + bool equals(const DecisionTree& other, double tol = 1e-9) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /** Make virtual */ - virtual ~DecisionTree() { - } + /** Make virtual */ + virtual ~DecisionTree() { + } - /** equality */ - bool operator==(const DecisionTree& q) const; + /** equality */ + bool operator==(const DecisionTree& q) const; - /** evaluate */ - const Y& operator()(const Assignment& x) const; + /** evaluate */ + const Y& operator()(const Assignment& x) const; - /** apply Unary operation "op" to f */ - DecisionTree apply(const Unary& op) const; + /** apply Unary operation "op" to f */ + DecisionTree apply(const Unary& op) const; - /** apply binary operation "op" to f and g */ - DecisionTree apply(const DecisionTree& g, const Binary& op) const; + /** apply binary operation "op" to f and g */ + DecisionTree apply(const DecisionTree& g, const Binary& op) const; - /** create a new function where value(label)==index */ - DecisionTree choose(const L& label, size_t index) const { - NodePtr newRoot = root_->choose(label, index); - return DecisionTree(newRoot); - } + /** create a new function where value(label)==index */ + DecisionTree choose(const L& label, size_t index) const { + NodePtr newRoot = root_->choose(label, index); + return DecisionTree(newRoot); + } - /** combine subtrees on key with binary operation "op" */ - DecisionTree combine(const L& label, size_t cardinality, const Binary& op) const; + /** combine subtrees on key with binary operation "op" */ + DecisionTree combine(const L& label, size_t cardinality, const Binary& op) const; - /** combine with LabelC for convenience */ - DecisionTree combine(const LabelC& labelC, const Binary& op) const { - return combine(labelC.first, labelC.second, op); - } + /** combine with LabelC for convenience */ + DecisionTree combine(const LabelC& labelC, const Binary& op) const { + return combine(labelC.first, labelC.second, op); + } - /** output to graphviz format, stream version */ - void dot(std::ostream& os, bool showZero = true) const; + /** output to graphviz format, stream version */ + void dot(std::ostream& os, bool showZero = true) const; - /** output to graphviz format, open a file */ - void dot(const std::string& name, bool showZero = true) const; + /** output to graphviz format, open a file */ + void dot(const std::string& name, bool showZero = true) const; - /// @name Advanced Interface - /// @{ + /// @name Advanced Interface + /// @{ - // internal use only - DecisionTree(const NodePtr& root); + // internal use only + DecisionTree(const NodePtr& root); - // internal use only - template NodePtr - compose(Iterator begin, Iterator end, const L& label) const; + // internal use only + template NodePtr + compose(Iterator begin, Iterator end, const L& label) const; - /// @} + /// @} - }; // DecisionTree + }; // DecisionTree - /** free versions of apply */ + /** free versions of apply */ - template - DecisionTree apply(const DecisionTree& f, - const typename DecisionTree::Unary& op) { - return f.apply(op); - } + template + DecisionTree apply(const DecisionTree& f, + const typename DecisionTree::Unary& op) { + return f.apply(op); + } - template - DecisionTree apply(const DecisionTree& f, - const DecisionTree& g, - const typename DecisionTree::Binary& op) { - return f.apply(g, op); - } + template + DecisionTree apply(const DecisionTree& f, + const DecisionTree& g, + const typename DecisionTree::Binary& op) { + return f.apply(g, op); + } } // namespace gtsam diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index bb83cec16..8c3e2e7b6 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -28,76 +28,76 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DecisionTreeFactor::DecisionTreeFactor() { - } + /* ******************************************************************************** */ + DecisionTreeFactor::DecisionTreeFactor() { + } - /* ******************************************************************************** */ - DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, - const ADT& potentials) : - DiscreteFactor(keys.indices()), Potentials(keys, potentials) { - } + /* ******************************************************************************** */ + DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, + const ADT& potentials) : + DiscreteFactor(keys.indices()), Potentials(keys, potentials) { + } - /* *************************************************************************/ - DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) : - DiscreteFactor(c.keys()), Potentials(c) { - } + /* *************************************************************************/ + DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) : + DiscreteFactor(c.keys()), Potentials(c) { + } - /* ************************************************************************* */ - bool DecisionTreeFactor::equals(const This& other, double tol) const { - return IndexFactor::equals(other, tol) && Potentials::equals(other, tol); - } + /* ************************************************************************* */ + bool DecisionTreeFactor::equals(const This& other, double tol) const { + return IndexFactor::equals(other, tol) && Potentials::equals(other, tol); + } - /* ************************************************************************* */ - void DecisionTreeFactor::print(const string& s, - const IndexFormatter& formatter) const { - cout << s; - IndexFactor::print("IndexFactor:",formatter); - Potentials::print("Potentials:",formatter); - } + /* ************************************************************************* */ + void DecisionTreeFactor::print(const string& s, + const IndexFormatter& formatter) const { + cout << s; + IndexFactor::print("IndexFactor:",formatter); + Potentials::print("Potentials:",formatter); + } - /* ************************************************************************* */ - DecisionTreeFactor DecisionTreeFactor::apply // - (const DecisionTreeFactor& f, ADT::Binary op) const { - map cs; // new cardinalities - // make unique key-cardinality map - BOOST_FOREACH(Index j, keys()) cs[j] = cardinality(j); - BOOST_FOREACH(Index j, f.keys()) cs[j] = f.cardinality(j); - // Convert map into keys - DiscreteKeys keys; - BOOST_FOREACH(const DiscreteKey& key, cs) - keys.push_back(key); - // apply operand - ADT result = ADT::apply(f, op); - // Make a new factor - return DecisionTreeFactor(keys, result); - } + /* ************************************************************************* */ + DecisionTreeFactor DecisionTreeFactor::apply // + (const DecisionTreeFactor& f, ADT::Binary op) const { + map cs; // new cardinalities + // make unique key-cardinality map + BOOST_FOREACH(Index j, keys()) cs[j] = cardinality(j); + BOOST_FOREACH(Index j, f.keys()) cs[j] = f.cardinality(j); + // Convert map into keys + DiscreteKeys keys; + BOOST_FOREACH(const DiscreteKey& key, cs) + keys.push_back(key); + // apply operand + ADT result = ADT::apply(f, op); + // Make a new factor + return DecisionTreeFactor(keys, result); + } - /* ************************************************************************* */ - DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine // - (size_t nrFrontals, ADT::Binary op) const { + /* ************************************************************************* */ + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine // + (size_t nrFrontals, ADT::Binary op) const { - if (nrFrontals == 0 || nrFrontals > size()) throw invalid_argument( - (boost::format( - "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d") - % nrFrontals % size()).str()); + if (nrFrontals == 0 || nrFrontals > size()) throw invalid_argument( + (boost::format( + "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d") + % nrFrontals % size()).str()); - // sum over nrFrontals keys - size_t i; - ADT result(*this); - for (i = 0; i < nrFrontals; i++) { - Index j = keys()[i]; - result = result.combine(j, cardinality(j), op); - } + // sum over nrFrontals keys + size_t i; + ADT result(*this); + for (i = 0; i < nrFrontals; i++) { + Index j = keys()[i]; + result = result.combine(j, cardinality(j), op); + } - // create new factor, note we start keys after nrFrontals - DiscreteKeys dkeys; - for (; i < keys().size(); i++) { - Index j = keys()[i]; - dkeys.push_back(DiscreteKey(j,cardinality(j))); - } - return boost::make_shared(dkeys, result); - } + // create new factor, note we start keys after nrFrontals + DiscreteKeys dkeys; + for (; i < keys().size(); i++) { + Index j = keys()[i]; + dkeys.push_back(DiscreteKey(j,cardinality(j))); + } + return boost::make_shared(dkeys, result); + } /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index f75c658a2..01d410ef0 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -30,126 +30,126 @@ namespace gtsam { - class DiscreteConditional; + class DiscreteConditional; - /** - * A discrete probabilistic factor - */ - class DecisionTreeFactor: public DiscreteFactor, public Potentials { + /** + * A discrete probabilistic factor + */ + class DecisionTreeFactor: public DiscreteFactor, public Potentials { - public: + public: - // typedefs needed to play nice with gtsam - typedef DecisionTreeFactor This; - typedef DiscreteConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + // typedefs needed to play nice with gtsam + typedef DecisionTreeFactor This; + typedef DiscreteConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; - public: + public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** Default constructor for I/O */ - DecisionTreeFactor(); + /** Default constructor for I/O */ + DecisionTreeFactor(); - /** Constructor from Indices, Ordering, and AlgebraicDecisionDiagram */ - DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials); + /** Constructor from Indices, Ordering, and AlgebraicDecisionDiagram */ + DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials); - /** Constructor from Indices and (string or doubles) */ - template - DecisionTreeFactor(const DiscreteKeys& keys, SOURCE table) : - DiscreteFactor(keys.indices()), Potentials(keys, table) { - } + /** Constructor from Indices and (string or doubles) */ + template + DecisionTreeFactor(const DiscreteKeys& keys, SOURCE table) : + DiscreteFactor(keys.indices()), Potentials(keys, table) { + } - /** Construct from a DiscreteConditional type */ - DecisionTreeFactor(const DiscreteConditional& c); + /** Construct from a DiscreteConditional type */ + DecisionTreeFactor(const DiscreteConditional& c); - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ - /// equality - bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; + /// equality + bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; - // print - virtual void print(const std::string& s = "DecisionTreeFactor:\n", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + // print + virtual void print(const std::string& s = "DecisionTreeFactor:\n", + const IndexFormatter& formatter = DefaultIndexFormatter) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /// Value is just look up in AlgebraicDecisonTree - virtual double operator()(const Values& values) const { - return Potentials::operator()(values); - } + /// Value is just look up in AlgebraicDecisonTree + virtual double operator()(const Values& values) const { + return Potentials::operator()(values); + } - /// multiply two factors - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { - return apply(f, ADT::Ring::mul); - } + /// multiply two factors + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const { + return apply(f, ADT::Ring::mul); + } - /// divide by factor f (safely) - DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { - return apply(f, safe_div); - } + /// divide by factor f (safely) + DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { + return apply(f, safe_div); + } - /// Convert into a decisiontree - virtual DecisionTreeFactor toDecisionTreeFactor() const { - return *this; - } + /// Convert into a decisiontree + virtual DecisionTreeFactor toDecisionTreeFactor() const { + return *this; + } - /// Create new factor by summing all values with the same separator values - shared_ptr sum(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::add); - } + /// Create new factor by summing all values with the same separator values + shared_ptr sum(size_t nrFrontals) const { + return combine(nrFrontals, ADT::Ring::add); + } - /// Create new factor by maximizing over all values with the same separator values - shared_ptr max(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::max); - } + /// Create new factor by maximizing over all values with the same separator values + shared_ptr max(size_t nrFrontals) const { + return combine(nrFrontals, ADT::Ring::max); + } - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - /** - * Apply binary operator (*this) "op" f - * @param f the second argument for op - * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials - */ - DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; + /** + * Apply binary operator (*this) "op" f + * @param f the second argument for op + * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials + */ + DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; - /** - * Combine frontal variables using binary operator "op" - * @param nrFrontals nr. of frontal to combine variables in this factor - * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials - * @return shared pointer to newly created DecisionTreeFactor - */ - shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; + /** + * Combine frontal variables using binary operator "op" + * @param nrFrontals nr. of frontal to combine variables in this factor + * @param op a binary operator that operates on AlgebraicDecisionDiagram potentials + * @return shared pointer to newly created DecisionTreeFactor + */ + shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; - /** - * @brief Permutes the keys in Potentials and DiscreteFactor - * - * This re-implements the permuteWithInverse() in both Potentials - * and DiscreteFactor by doing both of them together. - */ + /** + * @brief Permutes the keys in Potentials and DiscreteFactor + * + * This re-implements the permuteWithInverse() in both Potentials + * and DiscreteFactor by doing both of them together. + */ - void permuteWithInverse(const Permutation& inversePermutation){ - DiscreteFactor::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } + void permuteWithInverse(const Permutation& inversePermutation){ + DiscreteFactor::permuteWithInverse(inversePermutation); + Potentials::permuteWithInverse(inversePermutation); + } - /** - * Apply a reduction, which is a remapping of variable indices. - */ + /** + * Apply a reduction, which is a remapping of variable indices. + */ virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - DiscreteFactor::reduceWithInverse(inverseReduction); + DiscreteFactor::reduceWithInverse(inverseReduction); Potentials::reduceWithInverse(inverseReduction); } - /// @} - }; + /// @} + }; // DecisionTreeFactor }// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 1d21d0a4b..98a61f7bd 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -23,18 +23,18 @@ namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class BayesNet ; + // Explicitly instantiate so we don't have to include everywhere + template class BayesNet ; - /* ************************************************************************* */ - void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_front(boost::make_shared(s)); - } + /* ************************************************************************* */ + void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { + bayesNet.push_front(boost::make_shared(s)); + } - /* ************************************************************************* */ - void add(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_back(boost::make_shared(s)); - } + /* ************************************************************************* */ + void add(DiscreteBayesNet& bayesNet, const Signature& s) { + bayesNet.push_back(boost::make_shared(s)); + } /* ************************************************************************* */ double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values) { @@ -45,23 +45,23 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { - // solve each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, bn) - conditional->solveInPlace(*result); - return result; - } + /* ************************************************************************* */ + DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { + // solve each node in turn in topological sort order (parents first) + DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, bn) + conditional->solveInPlace(*result); + return result; + } - /* ************************************************************************* */ - DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn) { - // sample each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, bn) - conditional->sampleInPlace(*result); - return result; - } + /* ************************************************************************* */ + DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn) { + // sample each node in turn in topological sort order (parents first) + DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + conditional->sampleInPlace(*result); + return result; + } /* ************************************************************************* */ } // namespace diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index c8286f3a9..1e96f9919 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -25,22 +25,22 @@ namespace gtsam { - typedef BayesNet DiscreteBayesNet; + typedef BayesNet DiscreteBayesNet; - /** Add a DiscreteCondtional */ - void add(DiscreteBayesNet&, const Signature& s); + /** Add a DiscreteCondtional */ + void add(DiscreteBayesNet&, const Signature& s); - /** Add a DiscreteCondtional in front, when listing parents first*/ - void add_front(DiscreteBayesNet&, const Signature& s); + /** Add a DiscreteCondtional in front, when listing parents first*/ + void add_front(DiscreteBayesNet&, const Signature& s); - //** evaluate for given Values */ + //** evaluate for given Values */ double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values); /** Optimize function for back-substitution. */ - DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); + DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); - /** Do ancestral sampling */ - DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn); + /** Do ancestral sampling */ + DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn); } // namespace diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index b42e3ba24..2e03e5478 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -34,28 +34,28 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const size_t nrFrontals, - const DecisionTreeFactor& f) : - IndexConditional(f.keys(), nrFrontals), Potentials( - f / (*f.sum(nrFrontals))) { - } + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const size_t nrFrontals, + const DecisionTreeFactor& f) : + IndexConditional(f.keys(), nrFrontals), Potentials( + f / (*f.sum(nrFrontals))) { + } - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal) : - IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials( - ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { -// assert(nrFrontals() == 1); - if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout - << (firstFrontalKey()) << endl; //TODO Print all keys - } + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal) : + IndexConditional(joint.keys(), joint.size() - marginal.size()), Potentials( + ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { +// assert(nrFrontals() == 1); + if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout + << (firstFrontalKey()) << endl; //TODO Print all keys + } - /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const Signature& signature) : - IndexConditional(signature.indices(), 1), Potentials( - signature.discreteKeysParentsFirst(), signature.cpt()) { - } + /* ******************************************************************************** */ + DiscreteConditional::DiscreteConditional(const Signature& signature) : + IndexConditional(signature.indices(), 1), Potentials( + signature.discreteKeysParentsFirst(), signature.cpt()) { + } /* ******************************************************************************** */ void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const { @@ -70,145 +70,145 @@ namespace gtsam { && Potentials::equals(other, tol); } - /* ******************************************************************************** */ - Potentials::ADT DiscreteConditional::choose( - const Values& parentsValues) const { - ADT pFS(*this); - BOOST_FOREACH(Index key, parents()) - try { - Index j = (key); - size_t value = parentsValues.at(j); - pFS = pFS.choose(j, value); - } catch (exception&) { - throw runtime_error( - "DiscreteConditional::choose: parent value missing"); - }; - return pFS; - } + /* ******************************************************************************** */ + Potentials::ADT DiscreteConditional::choose( + const Values& parentsValues) const { + ADT pFS(*this); + BOOST_FOREACH(Index key, parents()) + try { + Index j = (key); + size_t value = parentsValues.at(j); + pFS = pFS.choose(j, value); + } catch (exception&) { + throw runtime_error( + "DiscreteConditional::choose: parent value missing"); + }; + return pFS; + } - /* ******************************************************************************** */ - void DiscreteConditional::solveInPlace(Values& values) const { -// OLD -// assert(nrFrontals() == 1); -// Index j = (firstFrontalKey()); -// size_t mpe = solve(values); // Solve for variable -// values[j] = mpe; // store result in partial solution -// OLD + /* ******************************************************************************** */ + void DiscreteConditional::solveInPlace(Values& values) const { +// OLD +// assert(nrFrontals() == 1); +// Index j = (firstFrontalKey()); +// size_t mpe = solve(values); // Solve for variable +// values[j] = mpe; // store result in partial solution +// OLD - // TODO: is this really the fastest way? I think it is. + // TODO: is this really the fastest way? I think it is. - //The following is to make make adjustment for nFrontals \neq 1 - ADT pFS = choose(values); // P(F|S=parentsValues) + //The following is to make make adjustment for nFrontals \neq 1 + ADT pFS = choose(values); // P(F|S=parentsValues) - // Initialize - Values mpe; - double maxP = 0; + // Initialize + Values mpe; + double maxP = 0; - DiscreteKeys keys; - BOOST_FOREACH(Index idx, frontals()) { - DiscreteKey dk(idx,cardinality(idx)); - keys & dk; - } - // Get all Possible Configurations - vector allPosbValues = cartesianProduct(keys); + DiscreteKeys keys; + BOOST_FOREACH(Index idx, frontals()) { + DiscreteKey dk(idx,cardinality(idx)); + keys & dk; + } + // Get all Possible Configurations + vector allPosbValues = cartesianProduct(keys); - // Find the MPE - BOOST_FOREACH(Values& frontalVals, allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } + // Find the MPE + BOOST_FOREACH(Values& frontalVals, allPosbValues) { + double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = frontalVals; + } + } - //set values (inPlace) to mpe - BOOST_FOREACH(Index j, frontals()) { - values[j] = mpe[j]; - } - } + //set values (inPlace) to mpe + BOOST_FOREACH(Index j, frontals()) { + values[j] = mpe[j]; + } + } - /* ******************************************************************************** */ - void DiscreteConditional::sampleInPlace(Values& values) const { - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - size_t sampled = sample(values); // Sample variable - values[j] = sampled; // store result in partial solution - } + /* ******************************************************************************** */ + void DiscreteConditional::sampleInPlace(Values& values) const { + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + size_t sampled = sample(values); // Sample variable + values[j] = sampled; // store result in partial solution + } - /* ******************************************************************************** */ - size_t DiscreteConditional::solve(const Values& parentsValues) const { + /* ******************************************************************************** */ + size_t DiscreteConditional::solve(const Values& parentsValues) const { - // TODO: is this really the fastest way? I think it is. - ADT pFS = choose(parentsValues); // P(F|S=parentsValues) + // TODO: is this really the fastest way? I think it is. + ADT pFS = choose(parentsValues); // P(F|S=parentsValues) - // Then, find the max over all remaining - // TODO, only works for one key now, seems horribly slow this way - size_t mpe = 0; - Values frontals; - double maxP = 0; - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - // Update MPE solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = value; - } - } - return mpe; - } + // Then, find the max over all remaining + // TODO, only works for one key now, seems horribly slow this way + size_t mpe = 0; + Values frontals; + double maxP = 0; + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + for (size_t value = 0; value < cardinality(j); value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + // Update MPE solution if better + if (pValueS > maxP) { + maxP = pValueS; + mpe = value; + } + } + return mpe; + } - /* ******************************************************************************** */ - size_t DiscreteConditional::sample(const Values& parentsValues) const { + /* ******************************************************************************** */ + size_t DiscreteConditional::sample(const Values& parentsValues) const { - using boost::uniform_real; - static boost::mt19937 gen(2); // random number generator + using boost::uniform_real; + static boost::mt19937 gen(2); // random number generator - bool debug = ISDEBUG("DiscreteConditional::sample"); + bool debug = ISDEBUG("DiscreteConditional::sample"); - // Get the correct conditional density - ADT pFS = choose(parentsValues); // P(F|S=parentsValues) - if (debug) GTSAM_PRINT(pFS); + // Get the correct conditional density + ADT pFS = choose(parentsValues); // P(F|S=parentsValues) + if (debug) GTSAM_PRINT(pFS); - // get cumulative distribution function (cdf) - // TODO, only works for one key now, seems horribly slow this way - assert(nrFrontals() == 1); - Index j = (firstFrontalKey()); - size_t nj = cardinality(j); - vector cdf(nj); - Values frontals; - double sum = 0; - for (size_t value = 0; value < nj; value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - sum += pValueS; // accumulate - if (debug) cout << sum << " "; - if (pValueS == 1) { - if (debug) cout << "--> " << value << endl; - return value; // shortcut exit - } - cdf[value] = sum; - } + // get cumulative distribution function (cdf) + // TODO, only works for one key now, seems horribly slow this way + assert(nrFrontals() == 1); + Index j = (firstFrontalKey()); + size_t nj = cardinality(j); + vector cdf(nj); + Values frontals; + double sum = 0; + for (size_t value = 0; value < nj; value++) { + frontals[j] = value; + double pValueS = pFS(frontals); // P(F=value|S=parentsValues) + sum += pValueS; // accumulate + if (debug) cout << sum << " "; + if (pValueS == 1) { + if (debug) cout << "--> " << value << endl; + return value; // shortcut exit + } + cdf[value] = sum; + } - // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html - uniform_real<> dist(0, cdf.back()); - boost::variate_generator > die(gen, dist); - size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin(); - if (debug) cout << "-> " << sampled << endl; + // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html + uniform_real<> dist(0, cdf.back()); + boost::variate_generator > die(gen, dist); + size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin(); + if (debug) cout << "-> " << sampled << endl; - return sampled; + return sampled; - return 0; - } + return 0; + } - /* ******************************************************************************** */ - void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ - IndexConditional::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } + /* ******************************************************************************** */ + void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ + IndexConditional::permuteWithInverse(inversePermutation); + Potentials::permuteWithInverse(inversePermutation); + } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index a11bdca45..7f5c99c62 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -26,52 +26,52 @@ namespace gtsam { - /** - * Discrete Conditional Density - * Derives from DecisionTreeFactor - */ - class DiscreteConditional: public IndexConditional, public Potentials { + /** + * Discrete Conditional Density + * Derives from DecisionTreeFactor + */ + class DiscreteConditional: public IndexConditional, public Potentials { - public: - // typedefs needed to play nice with gtsam - typedef DiscreteFactor FactorType; - typedef boost::shared_ptr shared_ptr; - typedef IndexConditional Base; + public: + // typedefs needed to play nice with gtsam + typedef DiscreteFactor FactorType; + typedef boost::shared_ptr shared_ptr; + typedef IndexConditional Base; - /** A map from keys to values */ - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values */ + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** default constructor needed for serialization */ - DiscreteConditional() { - } + /** default constructor needed for serialization */ + DiscreteConditional() { + } - /** constructor from factor */ - DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); + /** constructor from factor */ + DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); - /** Construct from signature */ - DiscreteConditional(const Signature& signature); + /** Construct from signature */ + DiscreteConditional(const Signature& signature); - /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ - DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal); + /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ + DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal); - /** - * Combine several conditional into a single one. - * The conditionals must be given in increasing order, meaning that the parents - * of any conditional may not include a conditional coming before it. - * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. - * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. - * */ - template - static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); + /** + * Combine several conditional into a single one. + * The conditionals must be given in increasing order, meaning that the parents + * of any conditional may not include a conditional coming before it. + * @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr. + * @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr. + * */ + template + static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional); - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ /// GTSAM-style print void print(const std::string& s = "Discrete Conditional: ", @@ -80,74 +80,74 @@ namespace gtsam { /// GTSAM-style equals bool equals(const DiscreteConditional& other, double tol = 1e-9) const; - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ /// Evaluate, just look up in AlgebraicDecisonTree virtual double operator()(const Values& values) const { return Potentials::operator()(values); } - /** Convert to a factor */ - DecisionTreeFactor::shared_ptr toFactor() const { - return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); - } + /** Convert to a factor */ + DecisionTreeFactor::shared_ptr toFactor() const { + return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); + } - /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ - ADT choose(const Assignment& parentsValues) const; + /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ + ADT choose(const Assignment& parentsValues) const; - /** - * solve a conditional - * @param parentsValues Known values of the parents - * @return MPE value of the child (1 frontal variable). - */ - size_t solve(const Values& parentsValues) const; + /** + * solve a conditional + * @param parentsValues Known values of the parents + * @return MPE value of the child (1 frontal variable). + */ + size_t solve(const Values& parentsValues) const; - /** - * sample - * @param parentsValues Known values of the parents - * @return sample from conditional - */ - size_t sample(const Values& parentsValues) const; + /** + * sample + * @param parentsValues Known values of the parents + * @return sample from conditional + */ + size_t sample(const Values& parentsValues) const; - /// @} - /// @name Advanced Interface - /// @{ + /// @} + /// @name Advanced Interface + /// @{ - /// solve a conditional, in place - void solveInPlace(Values& parentsValues) const; + /// solve a conditional, in place + void solveInPlace(Values& parentsValues) const; - /// sample in place, stores result in partial solution - void sampleInPlace(Values& parentsValues) const; + /// sample in place, stores result in partial solution + void sampleInPlace(Values& parentsValues) const; - /** - * Permutes both IndexConditional and Potentials. - */ - void permuteWithInverse(const Permutation& inversePermutation); + /** + * Permutes both IndexConditional and Potentials. + */ + void permuteWithInverse(const Permutation& inversePermutation); - /// @} + /// @} - }; + }; // DiscreteConditional - /* ************************************************************************* */ - template - DiscreteConditional::shared_ptr DiscreteConditional::Combine( - ITERATOR firstConditional, ITERATOR lastConditional) { - // TODO: check for being a clique + /* ************************************************************************* */ + template + DiscreteConditional::shared_ptr DiscreteConditional::Combine( + ITERATOR firstConditional, ITERATOR lastConditional) { + // TODO: check for being a clique - // multiply all the potentials of the given conditionals - size_t nrFrontals = 0; - DecisionTreeFactor product; - for(ITERATOR it = firstConditional; it != lastConditional; ++it, ++nrFrontals) { - DiscreteConditional::shared_ptr c = *it; - DecisionTreeFactor::shared_ptr factor = c->toFactor(); - product = (*factor) * product; - } - // and then create a new multi-frontal conditional - return boost::make_shared(nrFrontals,product); - } + // multiply all the potentials of the given conditionals + size_t nrFrontals = 0; + DecisionTreeFactor product; + for(ITERATOR it = firstConditional; it != lastConditional; ++it, ++nrFrontals) { + DiscreteConditional::shared_ptr c = *it; + DecisionTreeFactor::shared_ptr factor = c->toFactor(); + product = (*factor) * product; + } + // and then create a new multi-frontal conditional + return boost::make_shared(nrFrontals,product); + } }// gtsam diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index caec9788f..1d53f1afd 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -24,9 +24,9 @@ using namespace std; namespace gtsam { - /* ******************************************************************************** */ - DiscreteFactor::DiscreteFactor() { - } + /* ******************************************************************************** */ + DiscreteFactor::DiscreteFactor() { + } /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 2f411997c..499979eaf 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -23,100 +23,100 @@ namespace gtsam { - class DecisionTreeFactor; - class DiscreteConditional; + class DecisionTreeFactor; + class DiscreteConditional; - /** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor - */ - class DiscreteFactor: public IndexFactor { + /** + * Base class for discrete probabilistic factors + * The most general one is the derived DecisionTreeFactor + */ + class DiscreteFactor: public IndexFactor { - public: + public: - // typedefs needed to play nice with gtsam - typedef DiscreteFactor This; - typedef DiscreteConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + // typedefs needed to play nice with gtsam + typedef DiscreteFactor This; + typedef DiscreteConditional ConditionalType; + typedef boost::shared_ptr shared_ptr; - /** A map from keys to values */ - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values */ + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - protected: + protected: - /// Construct n-way factor - DiscreteFactor(const std::vector& js) : - IndexFactor(js) { - } + /// Construct n-way factor + DiscreteFactor(const std::vector& js) : + IndexFactor(js) { + } - /// Construct unary factor - DiscreteFactor(Index j) : - IndexFactor(j) { - } + /// Construct unary factor + DiscreteFactor(Index j) : + IndexFactor(j) { + } - /// Construct binary factor - DiscreteFactor(Index j1, Index j2) : - IndexFactor(j1, j2) { - } + /// Construct binary factor + DiscreteFactor(Index j1, Index j2) : + IndexFactor(j1, j2) { + } - /// construct from container - template - DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : - IndexFactor(beginKey, endKey) { - } + /// construct from container + template + DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : + IndexFactor(beginKey, endKey) { + } - public: + public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /// Default constructor for I/O - DiscreteFactor(); + /// Default constructor for I/O + DiscreteFactor(); - /// Virtual destructor - virtual ~DiscreteFactor() {} + /// Virtual destructor + virtual ~DiscreteFactor() {} - /// @} - /// @name Testable - /// @{ + /// @} + /// @name Testable + /// @{ - // print - virtual void print(const std::string& s = "DiscreteFactor\n", - const IndexFormatter& formatter - =DefaultIndexFormatter) const { - IndexFactor::print(s,formatter); - } + // print + virtual void print(const std::string& s = "DiscreteFactor\n", + const IndexFormatter& formatter + =DefaultIndexFormatter) const { + IndexFactor::print(s,formatter); + } - /// @} - /// @name Standard Interface - /// @{ + /// @} + /// @name Standard Interface + /// @{ - /// Find value for given assignment of values to variables - virtual double operator()(const Values&) const = 0; + /// Find value for given assignment of values to variables + virtual double operator()(const Values&) const = 0; - /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor - virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; + /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor + virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; - virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; + virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation){ - IndexFactor::permuteWithInverse(inversePermutation); - } + /** + * Permutes the factor, but for efficiency requires the permutation + * to already be inverted. + */ + virtual void permuteWithInverse(const Permutation& inversePermutation){ + IndexFactor::permuteWithInverse(inversePermutation); + } - /** - * Apply a reduction, which is a remapping of variable indices. - */ + /** + * Apply a reduction, which is a remapping of variable indices. + */ virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - IndexFactor::reduceWithInverse(inverseReduction); + IndexFactor::reduceWithInverse(inverseReduction); } - /// @} - }; + /// @} + }; // DiscreteFactor }// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 467611c5d..ab9cd1de2 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -24,56 +24,56 @@ namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class FactorGraph ; - template class EliminationTree ; + // Explicitly instantiate so we don't have to include everywhere + template class FactorGraph ; + template class EliminationTree ; - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph() { - } + /* ************************************************************************* */ + DiscreteFactorGraph::DiscreteFactorGraph() { + } - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph( - const BayesNet& bayesNet) : - FactorGraph(bayesNet) { - } + /* ************************************************************************* */ + DiscreteFactorGraph::DiscreteFactorGraph( + const BayesNet& bayesNet) : + FactorGraph(bayesNet) { + } - /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); - return keys; - } + /* ************************************************************************* */ + FastSet DiscreteFactorGraph::keys() const { + FastSet keys; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) keys.insert(factor->begin(), factor->end()); + return keys; + } - /* ************************************************************************* */ - DecisionTreeFactor DiscreteFactorGraph::product() const { - DecisionTreeFactor result; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) result = (*factor) * result; - return result; - } + /* ************************************************************************* */ + DecisionTreeFactor DiscreteFactorGraph::product() const { + DecisionTreeFactor result; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) result = (*factor) * result; + return result; + } - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteFactor::Values &values) const { - double product = 1.0; - BOOST_FOREACH( const sharedFactor& factor, factors_ ) - product *= (*factor)(values); - return product; - } + /* ************************************************************************* */ + double DiscreteFactorGraph::operator()( + const DiscreteFactor::Values &values) const { + double product = 1.0; + BOOST_FOREACH( const sharedFactor& factor, factors_ ) + product *= (*factor)(values); + return product; + } - /* ************************************************************************* */ - void DiscreteFactorGraph::print(const std::string& s, - const IndexFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); - } - } + /* ************************************************************************* */ + void DiscreteFactorGraph::print(const std::string& s, + const IndexFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + } + } /* ************************************************************************* */ void DiscreteFactorGraph::permuteWithInverse( @@ -93,32 +93,32 @@ namespace gtsam { } } - /* ************************************************************************* */ - std::pair // - EliminateDiscrete(const FactorGraph& factors, size_t num) { + /* ************************************************************************* */ + std::pair // + EliminateDiscrete(const FactorGraph& factors, size_t num) { - // PRODUCT: multiply all factors - tic(1, "product"); - DecisionTreeFactor product; - BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors){ - product = (*factor) * product; - } + // PRODUCT: multiply all factors + tic(1, "product"); + DecisionTreeFactor product; + BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors){ + product = (*factor) * product; + } - toc(1, "product"); + toc(1, "product"); - // sum out frontals, this is the factor on the separator - tic(2, "sum"); - DecisionTreeFactor::shared_ptr sum = product.sum(num); - toc(2, "sum"); + // sum out frontals, this is the factor on the separator + tic(2, "sum"); + DecisionTreeFactor::shared_ptr sum = product.sum(num); + toc(2, "sum"); - // now divide product/sum to get conditional - tic(3, "divide"); - DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum)); - toc(3, "divide"); - tictoc_finishedIteration(); + // now divide product/sum to get conditional + tic(3, "divide"); + DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum)); + toc(3, "divide"); + tictoc_finishedIteration(); - return std::make_pair(cond, sum); - } + return std::make_pair(cond, sum); + } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 323adca7b..f70f0840e 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -29,56 +29,56 @@ namespace gtsam { class DiscreteFactorGraph: public FactorGraph { public: - /** A map from keys to values */ - typedef std::vector Indices; - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /** A map from keys to values */ + typedef std::vector Indices; + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; - /** Construct empty factor graph */ - DiscreteFactorGraph(); + /** Construct empty factor graph */ + DiscreteFactorGraph(); - /** Constructor from a factor graph of GaussianFactor or a derived type */ - template - DiscreteFactorGraph(const FactorGraph& fg) { - push_back(fg); - } + /** Constructor from a factor graph of GaussianFactor or a derived type */ + template + DiscreteFactorGraph(const FactorGraph& fg) { + push_back(fg); + } - /** construct from a BayesNet */ - DiscreteFactorGraph(const BayesNet& bayesNet); + /** construct from a BayesNet */ + DiscreteFactorGraph(const BayesNet& bayesNet); - template - void add(const DiscreteKey& j, SOURCE table) { - DiscreteKeys keys; - keys.push_back(j); - push_back(boost::make_shared(keys, table)); - } + template + void add(const DiscreteKey& j, SOURCE table) { + DiscreteKeys keys; + keys.push_back(j); + push_back(boost::make_shared(keys, table)); + } - template - void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { - DiscreteKeys keys; - keys.push_back(j1); - keys.push_back(j2); - push_back(boost::make_shared(keys, table)); - } + template + void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { + DiscreteKeys keys; + keys.push_back(j1); + keys.push_back(j2); + push_back(boost::make_shared(keys, table)); + } - /** add shared discreteFactor immediately from arguments */ - template - void add(const DiscreteKeys& keys, SOURCE table) { - push_back(boost::make_shared(keys, table)); - } + /** add shared discreteFactor immediately from arguments */ + template + void add(const DiscreteKeys& keys, SOURCE table) { + push_back(boost::make_shared(keys, table)); + } - /** Return the set of variables involved in the factors (set union) */ - FastSet keys() const; + /** Return the set of variables involved in the factors (set union) */ + FastSet keys() const; - /** return product of all factors as a single factor */ - DecisionTreeFactor product() const; + /** return product of all factors as a single factor */ + DecisionTreeFactor product() const; - /** Evaluates the factor graph given values, returns the joint probability of the factor graph given specific instantiation of values*/ - double operator()(const DiscreteFactor::Values & values) const; + /** Evaluates the factor graph given values, returns the joint probability of the factor graph given specific instantiation of values*/ + double operator()(const DiscreteFactor::Values & values) const; - /// print - void print(const std::string& s = "DiscreteFactorGraph", - const IndexFormatter& formatter =DefaultIndexFormatter) const; + /// print + void print(const std::string& s = "DiscreteFactorGraph", + const IndexFormatter& formatter =DefaultIndexFormatter) const; /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); @@ -91,6 +91,6 @@ public: /** Main elimination function for DiscreteFactorGraph */ std::pair, DecisionTreeFactor::shared_ptr> EliminateDiscrete(const FactorGraph& factors, - size_t nrFrontals = 1); + size_t nrFrontals = 1); } // namespace gtsam diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index 9db8bee79..df9f5c3d9 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -23,33 +23,33 @@ namespace gtsam { - using namespace std; + using namespace std; - DiscreteKeys::DiscreteKeys(const vector& cs) { - for (size_t i = 0; i < cs.size(); i++) { - string name = boost::str(boost::format("v%1%") % i); - push_back(DiscreteKey(i, cs[i])); - } - } + DiscreteKeys::DiscreteKeys(const vector& cs) { + for (size_t i = 0; i < cs.size(); i++) { + string name = boost::str(boost::format("v%1%") % i); + push_back(DiscreteKey(i, cs[i])); + } + } - vector DiscreteKeys::indices() const { - vector < Index > js; - BOOST_FOREACH(const DiscreteKey& key, *this) - js.push_back(key.first); - return js; - } + vector DiscreteKeys::indices() const { + vector < Index > js; + BOOST_FOREACH(const DiscreteKey& key, *this) + js.push_back(key.first); + return js; + } - map DiscreteKeys::cardinalities() const { - map cs; - cs.insert(begin(),end()); -// BOOST_FOREACH(const DiscreteKey& key, *this) -// cs.insert(key); - return cs; - } + map DiscreteKeys::cardinalities() const { + map cs; + cs.insert(begin(),end()); +// BOOST_FOREACH(const DiscreteKey& key, *this) +// cs.insert(key); + return cs; + } - DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2) { - DiscreteKeys keys(key1); - return keys & key2; - } + DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2) { + DiscreteKeys keys(key1); + return keys & key2; + } } diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 488240286..96afba934 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -26,45 +26,45 @@ namespace gtsam { - /** - * Key type for discrete conditionals - * Includes name and cardinality - */ - typedef std::pair DiscreteKey; + /** + * Key type for discrete conditionals + * Includes name and cardinality + */ + typedef std::pair DiscreteKey; - /// DiscreteKeys is a set of keys that can be assembled using the & operator - struct DiscreteKeys: public std::vector { + /// DiscreteKeys is a set of keys that can be assembled using the & operator + struct DiscreteKeys: public std::vector { - /// Default constructor - DiscreteKeys() { - } + /// Default constructor + DiscreteKeys() { + } - /// Construct from a key - DiscreteKeys(const DiscreteKey& key) { - push_back(key); - } + /// Construct from a key + DiscreteKeys(const DiscreteKey& key) { + push_back(key); + } - /// Construct from a vector of keys - DiscreteKeys(const std::vector& keys) : - std::vector(keys) { - } + /// Construct from a vector of keys + DiscreteKeys(const std::vector& keys) : + std::vector(keys) { + } - /// Construct from cardinalities with default names - DiscreteKeys(const std::vector& cs); + /// Construct from cardinalities with default names + DiscreteKeys(const std::vector& cs); - /// Return a vector of indices - std::vector indices() const; + /// Return a vector of indices + std::vector indices() const; - /// Return a map from index to cardinality - std::map cardinalities() const; + /// Return a map from index to cardinality + std::map cardinalities() const; - /// Add a key (non-const!) - DiscreteKeys& operator&(const DiscreteKey& key) { - push_back(key); - return *this; - } - }; // DiscreteKeys + /// Add a key (non-const!) + DiscreteKeys& operator&(const DiscreteKey& key) { + push_back(key); + return *this; + } + }; // DiscreteKeys - /// Create a list from two keys - DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); + /// Create a list from two keys + DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); } diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index ff07e5a6c..411e4a4dd 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -36,41 +36,41 @@ namespace gtsam { public: - /** Construct a marginals class. - * @param graph The factor graph defining the full joint density on all variables. - */ - DiscreteMarginals(const DiscreteFactorGraph& graph) { - typedef JunctionTree DiscreteJT; - GenericMultifrontalSolver solver(graph); - bayesTree_ = *solver.eliminate(&EliminateDiscrete); - } + /** Construct a marginals class. + * @param graph The factor graph defining the full joint density on all variables. + */ + DiscreteMarginals(const DiscreteFactorGraph& graph) { + typedef JunctionTree DiscreteJT; + GenericMultifrontalSolver solver(graph); + bayesTree_ = *solver.eliminate(&EliminateDiscrete); + } - /** Compute the marginal of a single variable */ - DiscreteFactor::shared_ptr operator()(Index variable) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = bayesTree_.marginalFactor(variable, &EliminateDiscrete); - return marginalFactor; - } + /** Compute the marginal of a single variable */ + DiscreteFactor::shared_ptr operator()(Index variable) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor; + marginalFactor = bayesTree_.marginalFactor(variable, &EliminateDiscrete); + return marginalFactor; + } - /** Compute the marginal of a single variable - * @param key DiscreteKey of the Variable - * @return Vector of marginal probabilities - */ - Vector marginalProbabilities(const DiscreteKey& key) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = bayesTree_.marginalFactor(key.first, &EliminateDiscrete); + /** Compute the marginal of a single variable + * @param key DiscreteKey of the Variable + * @return Vector of marginal probabilities + */ + Vector marginalProbabilities(const DiscreteKey& key) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor; + marginalFactor = bayesTree_.marginalFactor(key.first, &EliminateDiscrete); - //Create result - Vector vResult(key.second); - for (size_t state = 0; state < key.second ; ++ state) { - DiscreteFactor::Values values; - values[key.first] = state; - vResult(state) = (*marginalFactor)(values); - } - return vResult; - } + //Create result + Vector vResult(key.second); + for (size_t state = 0; state < key.second ; ++ state) { + DiscreteFactor::Values values; + values[key.first] = state; + vResult(state) = (*marginalFactor)(values); + } + return vResult; + } }; diff --git a/gtsam/discrete/DiscreteSequentialSolver.cpp b/gtsam/discrete/DiscreteSequentialSolver.cpp index e4bc502c8..f01cd72bb 100644 --- a/gtsam/discrete/DiscreteSequentialSolver.cpp +++ b/gtsam/discrete/DiscreteSequentialSolver.cpp @@ -23,52 +23,52 @@ namespace gtsam { - template class GenericSequentialSolver ; + template class GenericSequentialSolver ; - /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteSequentialSolver::optimize() const { + /* ************************************************************************* */ + DiscreteFactor::sharedValues DiscreteSequentialSolver::optimize() const { - static const bool debug = false; + static const bool debug = false; - if (debug) this->factors_->print("DiscreteSequentialSolver, eliminating "); - if (debug) this->eliminationTree_->print( - "DiscreteSequentialSolver, elimination tree "); + if (debug) this->factors_->print("DiscreteSequentialSolver, eliminating "); + if (debug) this->eliminationTree_->print( + "DiscreteSequentialSolver, elimination tree "); - // Eliminate using the elimination tree - tic(1, "eliminate"); - DiscreteBayesNet::shared_ptr bayesNet = eliminate(); - toc(1, "eliminate"); + // Eliminate using the elimination tree + tic(1, "eliminate"); + DiscreteBayesNet::shared_ptr bayesNet = eliminate(); + toc(1, "eliminate"); - if (debug) bayesNet->print("DiscreteSequentialSolver, Bayes net "); + if (debug) bayesNet->print("DiscreteSequentialSolver, Bayes net "); - // Allocate the solution vector if it is not already allocated + // Allocate the solution vector if it is not already allocated - // Back-substitute - tic(2, "optimize"); - DiscreteFactor::sharedValues solution = gtsam::optimize(*bayesNet); - toc(2, "optimize"); + // Back-substitute + tic(2, "optimize"); + DiscreteFactor::sharedValues solution = gtsam::optimize(*bayesNet); + toc(2, "optimize"); - if (debug) solution->print("DiscreteSequentialSolver, solution "); + if (debug) solution->print("DiscreteSequentialSolver, solution "); - return solution; - } + return solution; + } - /* ************************************************************************* */ - Vector DiscreteSequentialSolver::marginalProbabilities( - const DiscreteKey& key) const { - // Compute marginal - DiscreteFactor::shared_ptr marginalFactor; - marginalFactor = Base::marginalFactor(key.first, &EliminateDiscrete); + /* ************************************************************************* */ + Vector DiscreteSequentialSolver::marginalProbabilities( + const DiscreteKey& key) const { + // Compute marginal + DiscreteFactor::shared_ptr marginalFactor; + marginalFactor = Base::marginalFactor(key.first, &EliminateDiscrete); - //Create result - Vector vResult(key.second); - for (size_t state = 0; state < key.second; ++state) { - DiscreteFactor::Values values; - values[key.first] = state; - vResult(state) = (*marginalFactor)(values); - } - return vResult; - } + //Create result + Vector vResult(key.second); + for (size_t state = 0; state < key.second; ++state) { + DiscreteFactor::Values values; + values[key.first] = state; + vResult(state) = (*marginalFactor)(values); + } + return vResult; + } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteSequentialSolver.h b/gtsam/discrete/DiscreteSequentialSolver.h index 6adb32ef6..fa1279bc8 100644 --- a/gtsam/discrete/DiscreteSequentialSolver.h +++ b/gtsam/discrete/DiscreteSequentialSolver.h @@ -23,91 +23,91 @@ #include namespace gtsam { - // The base class provides all of the needed functionality + // The base class provides all of the needed functionality - class DiscreteSequentialSolver: public GenericSequentialSolver { + class DiscreteSequentialSolver: public GenericSequentialSolver { - protected: - typedef GenericSequentialSolver Base; - typedef boost::shared_ptr shared_ptr; + protected: + typedef GenericSequentialSolver Base; + typedef boost::shared_ptr shared_ptr; - public: + public: - /** - * The problem we are trying to solve (SUM or MPE). - */ - typedef enum { - BEL, // Belief updating (or conditional updating) - MPE, // Most-Probable-Explanation - MAP - // Maximum A Posteriori hypothesis - } ProblemType; + /** + * The problem we are trying to solve (SUM or MPE). + */ + typedef enum { + BEL, // Belief updating (or conditional updating) + MPE, // Most-Probable-Explanation + MAP + // Maximum A Posteriori hypothesis + } ProblemType; - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - DiscreteSequentialSolver(const FactorGraph& factorGraph) : - Base(factorGraph) { - } + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the work of elimination. + */ + DiscreteSequentialSolver(const FactorGraph& factorGraph) : + Base(factorGraph) { + } - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - DiscreteSequentialSolver( - const FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex) : - Base(factorGraph, variableIndex) { - } + /** + * Construct the solver with a shared pointer to a factor graph and to a + * VariableIndex. The solver will store these pointers, so this constructor + * is the fastest. + */ + DiscreteSequentialSolver( + const FactorGraph::shared_ptr& factorGraph, + const VariableIndex::shared_ptr& variableIndex) : + Base(factorGraph, variableIndex) { + } - const EliminationTree& eliminationTree() const { - return *eliminationTree_; - } + const EliminationTree& eliminationTree() const { + return *eliminationTree_; + } - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - BayesNet::shared_ptr eliminate() const { - return Base::eliminate(&EliminateDiscrete); - } + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + BayesNet::shared_ptr eliminate() const { + return Base::eliminate(&EliminateDiscrete); + } - /** - * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. - */ - DiscreteFactorGraph::shared_ptr jointFactorGraph( - const std::vector& js) const { - DiscreteFactorGraph::shared_ptr results(new DiscreteFactorGraph( - *Base::jointFactorGraph(js, &EliminateDiscrete))); - return results; - } + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as a factor + * graph. + */ + DiscreteFactorGraph::shared_ptr jointFactorGraph( + const std::vector& js) const { + DiscreteFactorGraph::shared_ptr results(new DiscreteFactorGraph( + *Base::jointFactorGraph(js, &EliminateDiscrete))); + return results; + } - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - DiscreteFactor::shared_ptr marginalFactor(Index j) const { - return Base::marginalFactor(j, &EliminateDiscrete); - } + /** + * Compute the marginal density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + DiscreteFactor::shared_ptr marginalFactor(Index j) const { + return Base::marginalFactor(j, &EliminateDiscrete); + } - /** - * Compute the marginal density over a variable, by integrating out - * all of the other variables. This function returns the result as a - * Vector of the probability values. - */ - Vector marginalProbabilities(const DiscreteKey& key) const; + /** + * Compute the marginal density over a variable, by integrating out + * all of the other variables. This function returns the result as a + * Vector of the probability values. + */ + Vector marginalProbabilities(const DiscreteKey& key) const; - /** - * Compute the MPE solution of the DiscreteFactorGraph. This - * eliminates to create a BayesNet and then back-substitutes this BayesNet to - * obtain the solution. - */ - DiscreteFactor::sharedValues optimize() const; + /** + * Compute the MPE solution of the DiscreteFactorGraph. This + * eliminates to create a BayesNet and then back-substitutes this BayesNet to + * obtain the solution. + */ + DiscreteFactor::sharedValues optimize() const; - }; + }; } // gtsam diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index cf903a3a2..c8fb3a11d 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -23,42 +23,42 @@ using namespace std; namespace gtsam { - // explicit instantiation - template class DecisionTree ; - template class AlgebraicDecisionTree ; + // explicit instantiation + template class DecisionTree ; + template class AlgebraicDecisionTree ; - /* ************************************************************************* */ - double Potentials::safe_div(const double& a, const double& b) { - // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b)); - // The use for safe_div is when we divide the product factor by the sum factor. - // If the product or sum is zero, we accord zero probability to the event. - return (a == 0 || b == 0) ? 0 : (a / b); - } + /* ************************************************************************* */ + double Potentials::safe_div(const double& a, const double& b) { + // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b)); + // The use for safe_div is when we divide the product factor by the sum factor. + // If the product or sum is zero, we accord zero probability to the event. + return (a == 0 || b == 0) ? 0 : (a / b); + } - /* ******************************************************************************** */ - Potentials::Potentials() : - ADT(1.0) { - } + /* ******************************************************************************** */ + Potentials::Potentials() : + ADT(1.0) { + } - /* ******************************************************************************** */ - Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) : - ADT(decisionTree), cardinalities_(keys.cardinalities()) { - } + /* ******************************************************************************** */ + Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) : + ADT(decisionTree), cardinalities_(keys.cardinalities()) { + } - /* ************************************************************************* */ - bool Potentials::equals(const Potentials& other, double tol) const { - return ADT::equals(other, tol); - } + /* ************************************************************************* */ + bool Potentials::equals(const Potentials& other, double tol) const { + return ADT::equals(other, tol); + } - /* ************************************************************************* */ - void Potentials::print(const string& s, - const IndexFormatter& formatter) const { - cout << s << "\n Cardinalities: "; - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - cout << formatter(key.first) << "=" << formatter(key.second) << " "; - cout << endl; - ADT::print(" "); - } + /* ************************************************************************* */ + void Potentials::print(const string& s, + const IndexFormatter& formatter) const { + cout << s << "\n Cardinalities: "; + BOOST_FOREACH(const DiscreteKey& key, cardinalities_) + cout << formatter(key.first) << "=" << formatter(key.second) << " "; + cout << endl; + ADT::print(" "); + } /* ************************************************************************* */ template @@ -83,16 +83,16 @@ namespace gtsam { cardinalities_ = keys.cardinalities(); } - /* ************************************************************************* */ - void Potentials::permuteWithInverse(const Permutation& inversePermutation) { + /* ************************************************************************* */ + void Potentials::permuteWithInverse(const Permutation& inversePermutation) { remapIndices(inversePermutation); - } + } /* ************************************************************************* */ void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { remapIndices(inverseReduction); } - /* ************************************************************************* */ + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 5d8ace371..f2ba655b0 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -27,67 +27,67 @@ namespace gtsam { - /** - * A base class for both DiscreteFactor and DiscreteConditional - */ - class Potentials: public AlgebraicDecisionTree { + /** + * A base class for both DiscreteFactor and DiscreteConditional + */ + class Potentials: public AlgebraicDecisionTree { - public: + public: - typedef AlgebraicDecisionTree ADT; + typedef AlgebraicDecisionTree ADT; - protected: + protected: - /// Cardinality for each key, used in combine - std::map cardinalities_; + /// Cardinality for each key, used in combine + std::map cardinalities_; - /** Constructor from ColumnIndex, and ADT */ - Potentials(const ADT& potentials) : - ADT(potentials) { - } + /** Constructor from ColumnIndex, and ADT */ + Potentials(const ADT& potentials) : + ADT(potentials) { + } - // Safe division for probabilities - static double safe_div(const double& a, const double& b); + // Safe division for probabilities + static double safe_div(const double& a, const double& b); // Apply either a permutation or a reduction template void remapIndices(const P& remapping); - public: + public: - /** Default constructor for I/O */ - Potentials(); + /** Default constructor for I/O */ + Potentials(); - /** Constructor from Indices and ADT */ - Potentials(const DiscreteKeys& keys, const ADT& decisionTree); + /** Constructor from Indices and ADT */ + Potentials(const DiscreteKeys& keys, const ADT& decisionTree); - /** Constructor from Indices and (string or doubles) */ - template - Potentials(const DiscreteKeys& keys, SOURCE table) : - ADT(keys, table), cardinalities_(keys.cardinalities()) { - } + /** Constructor from Indices and (string or doubles) */ + template + Potentials(const DiscreteKeys& keys, SOURCE table) : + ADT(keys, table), cardinalities_(keys.cardinalities()) { + } - // Testable - bool equals(const Potentials& other, double tol = 1e-9) const; - void print(const std::string& s = "Potentials: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + // Testable + bool equals(const Potentials& other, double tol = 1e-9) const; + void print(const std::string& s = "Potentials: ", + const IndexFormatter& formatter = DefaultIndexFormatter) const; - size_t cardinality(Index j) const { return cardinalities_.at(j);} - - /** - * @brief Permutes the keys in Potentials - * - * This permutes the Indices and performs necessary re-ordering of ADD. - * This is virtual so that derived types e.g. DecisionTreeFactor can - * re-implement it. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation); + size_t cardinality(Index j) const { return cardinalities_.at(j);} /** - * Apply a reduction, which is a remapping of variable indices. - */ + * @brief Permutes the keys in Potentials + * + * This permutes the Indices and performs necessary re-ordering of ADD. + * This is virtual so that derived types e.g. DecisionTreeFactor can + * re-implement it. + */ + virtual void permuteWithInverse(const Permutation& inversePermutation); + + /** + * Apply a reduction, which is a remapping of variable indices. + */ virtual void reduceWithInverse(const internal::Reduction& inverseReduction); - }; // Potentials + }; // Potentials } // namespace gtsam diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 76ccad9db..006c851eb 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -26,197 +26,197 @@ namespace gtsam { - using namespace std; + using namespace std; - namespace qi = boost::spirit::qi; + namespace qi = boost::spirit::qi; namespace ph = boost::phoenix; - // parser for strings of form "99/1 80/20" etc... - namespace parser { - typedef string::const_iterator It; - using boost::phoenix::val; - using boost::phoenix::ref; - using boost::phoenix::push_back; + // parser for strings of form "99/1 80/20" etc... + namespace parser { + typedef string::const_iterator It; + using boost::phoenix::val; + using boost::phoenix::ref; + using boost::phoenix::push_back; - // Special rows, true and false - Signature::Row createF() { - Signature::Row r(2); - r[0] = 1; - r[1] = 0; - return r; - } - Signature::Row createT() { - Signature::Row r(2); - r[0] = 0; - r[1] = 1; - return r; - } - Signature::Row T = createT(), F = createF(); + // Special rows, true and false + Signature::Row createF() { + Signature::Row r(2); + r[0] = 1; + r[1] = 0; + return r; + } + Signature::Row createT() { + Signature::Row r(2); + r[0] = 0; + r[1] = 1; + return r; + } + Signature::Row T = createT(), F = createF(); - // Special tables (inefficient, but do we care for user input?) - Signature::Table logic(bool ff, bool ft, bool tf, bool tt) { - Signature::Table t(4); - t[0] = ff ? T : F; - t[1] = ft ? T : F; - t[2] = tf ? T : F; - t[3] = tt ? T : F; - return t; - } + // Special tables (inefficient, but do we care for user input?) + Signature::Table logic(bool ff, bool ft, bool tf, bool tt) { + Signature::Table t(4); + t[0] = ff ? T : F; + t[1] = ft ? T : F; + t[2] = tf ? T : F; + t[3] = tt ? T : F; + return t; + } - struct Grammar { - qi::rule table, or_, and_, rows; - qi::rule true_, false_, row; - Grammar() { - table = or_ | and_ | rows; - or_ = qi::lit("OR")[qi::_val = logic(false, true, true, true)]; - and_ = qi::lit("AND")[qi::_val = logic(false, false, false, true)]; - rows = +(row | true_ | false_); // only loads first of the rows under boost 1.42 - row = qi::double_ >> +("/" >> qi::double_); - true_ = qi::lit("T")[qi::_val = T]; - false_ = qi::lit("F")[qi::_val = F]; - } - } grammar; + struct Grammar { + qi::rule table, or_, and_, rows; + qi::rule true_, false_, row; + Grammar() { + table = or_ | and_ | rows; + or_ = qi::lit("OR")[qi::_val = logic(false, true, true, true)]; + and_ = qi::lit("AND")[qi::_val = logic(false, false, false, true)]; + rows = +(row | true_ | false_); // only loads first of the rows under boost 1.42 + row = qi::double_ >> +("/" >> qi::double_); + true_ = qi::lit("T")[qi::_val = T]; + false_ = qi::lit("F")[qi::_val = F]; + } + } grammar; - // Create simpler parsing function to avoid the issue of only parsing a single row - bool parse_table(const string& spec, Signature::Table& table) { - // check for OR, AND on whole phrase - It f = spec.begin(), l = spec.end(); - if (qi::parse(f, l, - qi::lit("OR")[ph::ref(table) = logic(false, true, true, true)]) || - qi::parse(f, l, - qi::lit("AND")[ph::ref(table) = logic(false, false, false, true)])) - return true; + // Create simpler parsing function to avoid the issue of only parsing a single row + bool parse_table(const string& spec, Signature::Table& table) { + // check for OR, AND on whole phrase + It f = spec.begin(), l = spec.end(); + if (qi::parse(f, l, + qi::lit("OR")[ph::ref(table) = logic(false, true, true, true)]) || + qi::parse(f, l, + qi::lit("AND")[ph::ref(table) = logic(false, false, false, true)])) + return true; - // tokenize into separate rows - istringstream iss(spec); - string token; - while (iss >> token) { - Signature::Row values; - It tf = token.begin(), tl = token.end(); - bool r = qi::parse(tf, tl, - qi::double_[push_back(ph::ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ph::ref(values), qi::_1)]) | - qi::lit("T")[ph::ref(values) = T] | - qi::lit("F")[ph::ref(values) = F] ); - if (!r) - return false; - table.push_back(values); - } + // tokenize into separate rows + istringstream iss(spec); + string token; + while (iss >> token) { + Signature::Row values; + It tf = token.begin(), tl = token.end(); + bool r = qi::parse(tf, tl, + qi::double_[push_back(ph::ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ph::ref(values), qi::_1)]) | + qi::lit("T")[ph::ref(values) = T] | + qi::lit("F")[ph::ref(values) = F] ); + if (!r) + return false; + table.push_back(values); + } - return true; - } - } // \namespace parser + return true; + } + } // \namespace parser - ostream& operator <<(ostream &os, const Signature::Row &row) { - os << row[0]; - for (size_t i = 1; i < row.size(); i++) - os << " " << row[i]; - return os; - } + ostream& operator <<(ostream &os, const Signature::Row &row) { + os << row[0]; + for (size_t i = 1; i < row.size(); i++) + os << " " << row[i]; + return os; + } - ostream& operator <<(ostream &os, const Signature::Table &table) { - for (size_t i = 0; i < table.size(); i++) - os << table[i] << endl; - return os; - } + ostream& operator <<(ostream &os, const Signature::Table &table) { + for (size_t i = 0; i < table.size(); i++) + os << table[i] << endl; + return os; + } - Signature::Signature(const DiscreteKey& key) : - key_(key) { - } + Signature::Signature(const DiscreteKey& key) : + key_(key) { + } - DiscreteKeys Signature::discreteKeysParentsFirst() const { - DiscreteKeys keys; - BOOST_FOREACH(const DiscreteKey& key, parents_) - keys.push_back(key); - keys.push_back(key_); - return keys; - } + DiscreteKeys Signature::discreteKeysParentsFirst() const { + DiscreteKeys keys; + BOOST_FOREACH(const DiscreteKey& key, parents_) + keys.push_back(key); + keys.push_back(key_); + return keys; + } - vector Signature::indices() const { - vector js; - js.push_back(key_.first); - BOOST_FOREACH(const DiscreteKey& key, parents_) - js.push_back(key.first); - return js; - } + vector Signature::indices() const { + vector js; + js.push_back(key_.first); + BOOST_FOREACH(const DiscreteKey& key, parents_) + js.push_back(key.first); + return js; + } - vector Signature::cpt() const { - vector cpt; - if (table_) { - BOOST_FOREACH(const Row& row, *table_) - BOOST_FOREACH(const double& x, row) - cpt.push_back(x); - } - return cpt; - } + vector Signature::cpt() const { + vector cpt; + if (table_) { + BOOST_FOREACH(const Row& row, *table_) + BOOST_FOREACH(const double& x, row) + cpt.push_back(x); + } + return cpt; + } - Signature& Signature::operator,(const DiscreteKey& parent) { - parents_.push_back(parent); - return *this; - } + Signature& Signature::operator,(const DiscreteKey& parent) { + parents_.push_back(parent); + return *this; + } - static void normalize(Signature::Row& row) { - double sum = 0; - for (size_t i = 0; i < row.size(); i++) - sum += row[i]; - for (size_t i = 0; i < row.size(); i++) - row[i] /= sum; - } + static void normalize(Signature::Row& row) { + double sum = 0; + for (size_t i = 0; i < row.size(); i++) + sum += row[i]; + for (size_t i = 0; i < row.size(); i++) + row[i] /= sum; + } - Signature& Signature::operator=(const string& spec) { - spec_.reset(spec); - Table table; - // NOTE: using simpler parse function to ensure boost back compatibility -// parser::It f = spec.begin(), l = spec.end(); - bool success = // -// qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar - parser::parse_table(spec, table); - if (success) { - BOOST_FOREACH(Row& row, table) - normalize(row); - table_.reset(table); - } - return *this; - } + Signature& Signature::operator=(const string& spec) { + spec_.reset(spec); + Table table; + // NOTE: using simpler parse function to ensure boost back compatibility +// parser::It f = spec.begin(), l = spec.end(); + bool success = // +// qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar + parser::parse_table(spec, table); + if (success) { + BOOST_FOREACH(Row& row, table) + normalize(row); + table_.reset(table); + } + return *this; + } - Signature& Signature::operator=(const Table& t) { - Table table = t; - BOOST_FOREACH(Row& row, table) - normalize(row); - table_.reset(table); - return *this; - } + Signature& Signature::operator=(const Table& t) { + Table table = t; + BOOST_FOREACH(Row& row, table) + normalize(row); + table_.reset(table); + return *this; + } - ostream& operator <<(ostream &os, const Signature &s) { - os << s.key_.first; - if (s.parents_.empty()) { - os << " % "; - } else { - os << " | " << s.parents_[0].first; - for (size_t i = 1; i < s.parents_.size(); i++) - os << " && " << s.parents_[i].first; - os << " = "; - } - os << (s.spec_ ? *s.spec_ : "no spec") << endl; - if (s.table_) - os << (*s.table_); - else - os << "spec could not be parsed" << endl; - return os; - } + ostream& operator <<(ostream &os, const Signature &s) { + os << s.key_.first; + if (s.parents_.empty()) { + os << " % "; + } else { + os << " | " << s.parents_[0].first; + for (size_t i = 1; i < s.parents_.size(); i++) + os << " && " << s.parents_[i].first; + os << " = "; + } + os << (s.spec_ ? *s.spec_ : "no spec") << endl; + if (s.table_) + os << (*s.table_); + else + os << "spec could not be parsed" << endl; + return os; + } - Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) { - Signature s(key); - return s, parent; - } + Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) { + Signature s(key); + return s, parent; + } - Signature operator%(const DiscreteKey& key, const string& parent) { - Signature s(key); - return s = parent; - } + Signature operator%(const DiscreteKey& key, const string& parent) { + Signature s(key); + return s = parent; + } - Signature operator%(const DiscreteKey& key, const Signature::Table& parent) { - Signature s(key); - return s = parent; - } + Signature operator%(const DiscreteKey& key, const Signature::Table& parent) { + Signature s(key); + return s = parent; + } } // namespace gtsam diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index 937bd6e1b..0b3d0879d 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -24,112 +24,112 @@ namespace gtsam { - /** - * Signature for a discrete conditional density, used to construct conditionals. - * - * The format is (Key % string) for nodes with no parents, - * and (Key | Key, Key = string) for nodes with parents. - * - * The string specifies a conditional probability spec in the 00 01 10 11 order. - * For three-valued, it would be 00 01 02 10 11 12 20 21 22, etc... - * - * For example, given the following keys - * - * Key A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), - * B("Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea"); - * - * These are all valid signatures (Asia network example): - * - * A % "99/1" - * S % "50/50" - * T|A = "99/1 95/5" - * L|S = "99/1 90/10" - * B|S = "70/30 40/60" - * E|T,L = "F F F 1" - * X|E = "95/5 2/98" - * D|E,B = "9/1 2/8 3/7 1/9" - */ - class Signature { + /** + * Signature for a discrete conditional density, used to construct conditionals. + * + * The format is (Key % string) for nodes with no parents, + * and (Key | Key, Key = string) for nodes with parents. + * + * The string specifies a conditional probability spec in the 00 01 10 11 order. + * For three-valued, it would be 00 01 02 10 11 12 20 21 22, etc... + * + * For example, given the following keys + * + * Key A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), + * B("Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea"); + * + * These are all valid signatures (Asia network example): + * + * A % "99/1" + * S % "50/50" + * T|A = "99/1 95/5" + * L|S = "99/1 90/10" + * B|S = "70/30 40/60" + * E|T,L = "F F F 1" + * X|E = "95/5 2/98" + * D|E,B = "9/1 2/8 3/7 1/9" + */ + class Signature { - public: + public: - /** Data type for the CPT */ - typedef std::vector Row; - typedef std::vector Table; + /** Data type for the CPT */ + typedef std::vector Row; + typedef std::vector Table; - private: + private: - /** the variable key */ - DiscreteKey key_; + /** the variable key */ + DiscreteKey key_; - /** the parent keys */ - DiscreteKeys parents_; + /** the parent keys */ + DiscreteKeys parents_; - // the given CPT specification string - boost::optional spec_; + // the given CPT specification string + boost::optional spec_; - // the CPT as parsed, if successful - boost::optional table_; + // the CPT as parsed, if successful + boost::optional
table_; - public: + public: - /** Constructor from DiscreteKey */ - Signature(const DiscreteKey& key); + /** Constructor from DiscreteKey */ + Signature(const DiscreteKey& key); - /** the variable key */ - const DiscreteKey& key() const { - return key_; - } + /** the variable key */ + const DiscreteKey& key() const { + return key_; + } - /** the parent keys */ - const DiscreteKeys& parents() const { - return parents_; - } + /** the parent keys */ + const DiscreteKeys& parents() const { + return parents_; + } - /** All keys, with variable key last */ - DiscreteKeys discreteKeysParentsFirst() const; + /** All keys, with variable key last */ + DiscreteKeys discreteKeysParentsFirst() const; - /** All key indices, with variable key first */ - std::vector indices() const; + /** All key indices, with variable key first */ + std::vector indices() const; - // the CPT as parsed, if successful - const boost::optional
& table() const { - return table_; - } + // the CPT as parsed, if successful + const boost::optional
& table() const { + return table_; + } - // the CPT as a vector of doubles, with key's values most rapidly changing - std::vector cpt() const; + // the CPT as a vector of doubles, with key's values most rapidly changing + std::vector cpt() const; - /** Add a parent */ - Signature& operator,(const DiscreteKey& parent); + /** Add a parent */ + Signature& operator,(const DiscreteKey& parent); - /** Add the CPT spec - Fails in boost 1.40 */ - Signature& operator=(const std::string& spec); + /** Add the CPT spec - Fails in boost 1.40 */ + Signature& operator=(const std::string& spec); - /** Add the CPT spec directly as a table */ - Signature& operator=(const Table& table); + /** Add the CPT spec directly as a table */ + Signature& operator=(const Table& table); - /** provide streaming */ - friend std::ostream& operator <<(std::ostream &os, const Signature &s); - }; + /** provide streaming */ + friend std::ostream& operator <<(std::ostream &os, const Signature &s); + }; - /** - * Helper function to create Signature objects - * example: Signature s = D | E; - */ - Signature operator|(const DiscreteKey& key, const DiscreteKey& parent); + /** + * Helper function to create Signature objects + * example: Signature s = D | E; + */ + Signature operator|(const DiscreteKey& key, const DiscreteKey& parent); - /** - * Helper function to create Signature objects - * example: Signature s(D % "99/1"); - * Uses string parser, which requires BOOST 1.42 or higher - */ - Signature operator%(const DiscreteKey& key, const std::string& parent); + /** + * Helper function to create Signature objects + * example: Signature s(D % "99/1"); + * Uses string parser, which requires BOOST 1.42 or higher + */ + Signature operator%(const DiscreteKey& key, const std::string& parent); - /** - * Helper function to create Signature objects, using table construction directly - * example: Signature s(D % table); - */ - Signature operator%(const DiscreteKey& key, const Signature::Table& parent); + /** + * Helper function to create Signature objects, using table construction directly + * example: Signature s(D % table); + */ + Signature operator%(const DiscreteKey& key, const Signature::Table& parent); } diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 8ab3b6a14..d0cd548a7 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /* - * @file testDecisionTree.cpp - * @brief Develop DecisionTree - * @author Frank Dellaert - * @date Mar 6, 2011 + * @file testDecisionTree.cpp + * @brief Develop DecisionTree + * @author Frank Dellaert + * @date Mar 6, 2011 */ #include @@ -48,7 +48,7 @@ template class AlgebraicDecisionTree; template void dot(const T&f, const string& filename) { #ifndef DISABLE_DOT - f.dot(filename); + f.dot(filename); #endif } @@ -76,54 +76,54 @@ void dot(const T&f, const string& filename) { size_t muls = 0, adds = 0; boost::timer timer; void resetCounts() { - muls = 0; - adds = 0; - timer.restart(); + muls = 0; + adds = 0; + timer.restart(); } void printCounts(const string& s) { #ifndef DISABLE_TIMING - cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds - % (1000 * timer.elapsed()) << endl; + cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds + % (1000 * timer.elapsed()) << endl; #endif - resetCounts(); + resetCounts(); } double mul(const double& a, const double& b) { - muls++; - return a * b; + muls++; + return a * b; } double add_(const double& a, const double& b) { - adds++; - return a + b; + adds++; + return a + b; } /* ******************************************************************************** */ // test ADT TEST(ADT, example3) { - // Create labels - DiscreteKey A(0,2), B(1,2), C(2,2), D(3,2), E(4,2); + // Create labels + DiscreteKey A(0,2), B(1,2), C(2,2), D(3,2), E(4,2); - // Literals - ADT a(A, 0.5, 0.5); - ADT notb(B, 1, 0); - ADT c(C, 0.1, 0.9); - ADT d(D, 0.1, 0.9); - ADT note(E, 0.9, 0.1); + // Literals + ADT a(A, 0.5, 0.5); + ADT notb(B, 1, 0); + ADT c(C, 0.1, 0.9); + ADT d(D, 0.1, 0.9); + ADT note(E, 0.9, 0.1); - ADT cnotb = c * notb; - dot(cnotb, "ADT-cnotb"); + ADT cnotb = c * notb; + dot(cnotb, "ADT-cnotb"); // a.print("a: "); // cnotb.print("cnotb: "); - ADT acnotb = a * cnotb; -// acnotb.print("acnotb: "); -// acnotb.printCache("acnotb Cache:"); + ADT acnotb = a * cnotb; +// acnotb.print("acnotb: "); +// acnotb.printCache("acnotb Cache:"); - dot(acnotb, "ADT-acnotb"); + dot(acnotb, "ADT-acnotb"); - ADT big = apply(apply(d, note, &mul), acnotb, &add_); - dot(big, "ADT-big"); + ADT big = apply(apply(d, note, &mul), acnotb, &add_); + dot(big, "ADT-big"); } /* ******************************************************************************** */ @@ -132,238 +132,238 @@ TEST(ADT, example3) /** Convert Signature into CPT */ ADT create(const Signature& signature) { - ADT p(signature.discreteKeysParentsFirst(), signature.cpt()); - static size_t count = 0; - const DiscreteKey& key = signature.key(); - string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); - dot(p, dotfile); - return p; + ADT p(signature.discreteKeysParentsFirst(), signature.cpt()); + static size_t count = 0; + const DiscreteKey& key = signature.key(); + string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); + dot(p, dotfile); + return p; } /* ************************************************************************* */ // test Asia Joint TEST(ADT, joint) { - DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2); + DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2); - resetCounts(); - ADT pA = create(A % "99/1"); - ADT pS = create(S % "50/50"); - ADT pT = create(T | A = "99/1 95/5"); - ADT pL = create(L | S = "99/1 90/10"); - ADT pB = create(B | S = "70/30 40/60"); - ADT pE = create((E | T, L) = "F T T T"); - ADT pX = create(X | E = "95/5 2/98"); - ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); - printCounts("Asia CPTs"); + resetCounts(); + ADT pA = create(A % "99/1"); + ADT pS = create(S % "50/50"); + ADT pT = create(T | A = "99/1 95/5"); + ADT pL = create(L | S = "99/1 90/10"); + ADT pB = create(B | S = "70/30 40/60"); + ADT pE = create((E | T, L) = "F T T T"); + ADT pX = create(X | E = "95/5 2/98"); + ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); + printCounts("Asia CPTs"); - // Create joint - resetCounts(); - ADT joint = pA; - dot(joint, "Asia-A"); - joint = apply(joint, pS, &mul); - dot(joint, "Asia-AS"); - joint = apply(joint, pT, &mul); - dot(joint, "Asia-AST"); - joint = apply(joint, pL, &mul); - dot(joint, "Asia-ASTL"); - joint = apply(joint, pB, &mul); - dot(joint, "Asia-ASTLB"); - joint = apply(joint, pE, &mul); - dot(joint, "Asia-ASTLBE"); - joint = apply(joint, pX, &mul); - dot(joint, "Asia-ASTLBEX"); - joint = apply(joint, pD, &mul); - dot(joint, "Asia-ASTLBEXD"); - EXPECT_LONGS_EQUAL(346, muls); - printCounts("Asia joint"); + // Create joint + resetCounts(); + ADT joint = pA; + dot(joint, "Asia-A"); + joint = apply(joint, pS, &mul); + dot(joint, "Asia-AS"); + joint = apply(joint, pT, &mul); + dot(joint, "Asia-AST"); + joint = apply(joint, pL, &mul); + dot(joint, "Asia-ASTL"); + joint = apply(joint, pB, &mul); + dot(joint, "Asia-ASTLB"); + joint = apply(joint, pE, &mul); + dot(joint, "Asia-ASTLBE"); + joint = apply(joint, pX, &mul); + dot(joint, "Asia-ASTLBEX"); + joint = apply(joint, pD, &mul); + dot(joint, "Asia-ASTLBEXD"); + EXPECT_LONGS_EQUAL(346, muls); + printCounts("Asia joint"); - ADT pASTL = pA; - pASTL = apply(pASTL, pS, &mul); - pASTL = apply(pASTL, pT, &mul); - pASTL = apply(pASTL, pL, &mul); + ADT pASTL = pA; + pASTL = apply(pASTL, pS, &mul); + pASTL = apply(pASTL, pT, &mul); + pASTL = apply(pASTL, pL, &mul); - // test combine - ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_); - EXPECT(assert_equal(pA, fAa)); - ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_); - EXPECT(assert_equal(pA, fAb)); + // test combine + ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_); + EXPECT(assert_equal(pA, fAa)); + ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_); + EXPECT(assert_equal(pA, fAb)); } /* ************************************************************************* */ // test Inference with joint TEST(ADT, inference) { - DiscreteKey A(0,2), D(1,2),// - B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2); + DiscreteKey A(0,2), D(1,2),// + B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2); - resetCounts(); - ADT pA = create(A % "99/1"); - ADT pS = create(S % "50/50"); - ADT pT = create(T | A = "99/1 95/5"); - ADT pL = create(L | S = "99/1 90/10"); - ADT pB = create(B | S = "70/30 40/60"); - ADT pE = create((E | T, L) = "F T T T"); - ADT pX = create(X | E = "95/5 2/98"); - ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); - // printCounts("Inference CPTs"); + resetCounts(); + ADT pA = create(A % "99/1"); + ADT pS = create(S % "50/50"); + ADT pT = create(T | A = "99/1 95/5"); + ADT pL = create(L | S = "99/1 90/10"); + ADT pB = create(B | S = "70/30 40/60"); + ADT pE = create((E | T, L) = "F T T T"); + ADT pX = create(X | E = "95/5 2/98"); + ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); + // printCounts("Inference CPTs"); - // Create joint - resetCounts(); - ADT joint = pA; - dot(joint, "Joint-Product-A"); - joint = apply(joint, pS, &mul); - dot(joint, "Joint-Product-AS"); - joint = apply(joint, pT, &mul); - dot(joint, "Joint-Product-AST"); - joint = apply(joint, pL, &mul); - dot(joint, "Joint-Product-ASTL"); - joint = apply(joint, pB, &mul); - dot(joint, "Joint-Product-ASTLB"); - joint = apply(joint, pE, &mul); - dot(joint, "Joint-Product-ASTLBE"); - joint = apply(joint, pX, &mul); - dot(joint, "Joint-Product-ASTLBEX"); - joint = apply(joint, pD, &mul); - dot(joint, "Joint-Product-ASTLBEXD"); - EXPECT_LONGS_EQUAL(370, muls); // different ordering - printCounts("Asia product"); + // Create joint + resetCounts(); + ADT joint = pA; + dot(joint, "Joint-Product-A"); + joint = apply(joint, pS, &mul); + dot(joint, "Joint-Product-AS"); + joint = apply(joint, pT, &mul); + dot(joint, "Joint-Product-AST"); + joint = apply(joint, pL, &mul); + dot(joint, "Joint-Product-ASTL"); + joint = apply(joint, pB, &mul); + dot(joint, "Joint-Product-ASTLB"); + joint = apply(joint, pE, &mul); + dot(joint, "Joint-Product-ASTLBE"); + joint = apply(joint, pX, &mul); + dot(joint, "Joint-Product-ASTLBEX"); + joint = apply(joint, pD, &mul); + dot(joint, "Joint-Product-ASTLBEXD"); + EXPECT_LONGS_EQUAL(370, muls); // different ordering + printCounts("Asia product"); - ADT marginal = joint; - marginal = marginal.combine(X, &add_); - dot(marginal, "Joint-Sum-ADBLEST"); - marginal = marginal.combine(T, &add_); - dot(marginal, "Joint-Sum-ADBLES"); - marginal = marginal.combine(S, &add_); - dot(marginal, "Joint-Sum-ADBLE"); - marginal = marginal.combine(E, &add_); - dot(marginal, "Joint-Sum-ADBL"); - EXPECT_LONGS_EQUAL(161, adds); - printCounts("Asia sum"); + ADT marginal = joint; + marginal = marginal.combine(X, &add_); + dot(marginal, "Joint-Sum-ADBLEST"); + marginal = marginal.combine(T, &add_); + dot(marginal, "Joint-Sum-ADBLES"); + marginal = marginal.combine(S, &add_); + dot(marginal, "Joint-Sum-ADBLE"); + marginal = marginal.combine(E, &add_); + dot(marginal, "Joint-Sum-ADBL"); + EXPECT_LONGS_EQUAL(161, adds); + printCounts("Asia sum"); } /* ************************************************************************* */ TEST(ADT, factor_graph) { - DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2); + DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2); - resetCounts(); - ADT pS = create(S % "50/50"); - ADT pT = create(T % "95/5"); - ADT pL = create(L | S = "99/1 90/10"); - ADT pE = create((E | T, L) = "F T T T"); - ADT pX = create(X | E = "95/5 2/98"); - ADT pD = create(B | E = "1/8 7/9"); - ADT pB = create(B | S = "70/30 40/60"); - // printCounts("Create CPTs"); + resetCounts(); + ADT pS = create(S % "50/50"); + ADT pT = create(T % "95/5"); + ADT pL = create(L | S = "99/1 90/10"); + ADT pE = create((E | T, L) = "F T T T"); + ADT pX = create(X | E = "95/5 2/98"); + ADT pD = create(B | E = "1/8 7/9"); + ADT pB = create(B | S = "70/30 40/60"); + // printCounts("Create CPTs"); - // Create joint - resetCounts(); - ADT fg = pS; - fg = apply(fg, pT, &mul); - fg = apply(fg, pL, &mul); - fg = apply(fg, pB, &mul); - fg = apply(fg, pE, &mul); - fg = apply(fg, pX, &mul); - fg = apply(fg, pD, &mul); - dot(fg, "FactorGraph"); - EXPECT_LONGS_EQUAL(158, muls); - printCounts("Asia FG"); + // Create joint + resetCounts(); + ADT fg = pS; + fg = apply(fg, pT, &mul); + fg = apply(fg, pL, &mul); + fg = apply(fg, pB, &mul); + fg = apply(fg, pE, &mul); + fg = apply(fg, pX, &mul); + fg = apply(fg, pD, &mul); + dot(fg, "FactorGraph"); + EXPECT_LONGS_EQUAL(158, muls); + printCounts("Asia FG"); - fg = fg.combine(X, &add_); - dot(fg, "Marginalized-6X"); - fg = fg.combine(T, &add_); - dot(fg, "Marginalized-5T"); - fg = fg.combine(S, &add_); - dot(fg, "Marginalized-4S"); - fg = fg.combine(E, &add_); - dot(fg, "Marginalized-3E"); - fg = fg.combine(L, &add_); - dot(fg, "Marginalized-2L"); - EXPECT(adds = 54); - printCounts("marginalize"); + fg = fg.combine(X, &add_); + dot(fg, "Marginalized-6X"); + fg = fg.combine(T, &add_); + dot(fg, "Marginalized-5T"); + fg = fg.combine(S, &add_); + dot(fg, "Marginalized-4S"); + fg = fg.combine(E, &add_); + dot(fg, "Marginalized-3E"); + fg = fg.combine(L, &add_); + dot(fg, "Marginalized-2L"); + EXPECT(adds = 54); + printCounts("marginalize"); - // BLESTX + // BLESTX - // Eliminate X - ADT fE = pX; - dot(fE, "Eliminate-01-fEX"); - fE = fE.combine(X, &add_); - dot(fE, "Eliminate-02-fE"); - printCounts("Eliminate X"); + // Eliminate X + ADT fE = pX; + dot(fE, "Eliminate-01-fEX"); + fE = fE.combine(X, &add_); + dot(fE, "Eliminate-02-fE"); + printCounts("Eliminate X"); - // Eliminate T - ADT fLE = pT; - fLE = apply(fLE, pE, &mul); - dot(fLE, "Eliminate-03-fLET"); - fLE = fLE.combine(T, &add_); - dot(fLE, "Eliminate-04-fLE"); - printCounts("Eliminate T"); + // Eliminate T + ADT fLE = pT; + fLE = apply(fLE, pE, &mul); + dot(fLE, "Eliminate-03-fLET"); + fLE = fLE.combine(T, &add_); + dot(fLE, "Eliminate-04-fLE"); + printCounts("Eliminate T"); - // Eliminate S - ADT fBL = pS; - fBL = apply(fBL, pL, &mul); - fBL = apply(fBL, pB, &mul); - dot(fBL, "Eliminate-05-fBLS"); - fBL = fBL.combine(S, &add_); - dot(fBL, "Eliminate-06-fBL"); - printCounts("Eliminate S"); + // Eliminate S + ADT fBL = pS; + fBL = apply(fBL, pL, &mul); + fBL = apply(fBL, pB, &mul); + dot(fBL, "Eliminate-05-fBLS"); + fBL = fBL.combine(S, &add_); + dot(fBL, "Eliminate-06-fBL"); + printCounts("Eliminate S"); - // Eliminate E - ADT fBL2 = fE; - fBL2 = apply(fBL2, fLE, &mul); - fBL2 = apply(fBL2, pD, &mul); - dot(fBL2, "Eliminate-07-fBLE"); - fBL2 = fBL2.combine(E, &add_); - dot(fBL2, "Eliminate-08-fBL2"); - printCounts("Eliminate E"); + // Eliminate E + ADT fBL2 = fE; + fBL2 = apply(fBL2, fLE, &mul); + fBL2 = apply(fBL2, pD, &mul); + dot(fBL2, "Eliminate-07-fBLE"); + fBL2 = fBL2.combine(E, &add_); + dot(fBL2, "Eliminate-08-fBL2"); + printCounts("Eliminate E"); - // Eliminate L - ADT fB = fBL; - fB = apply(fB, fBL2, &mul); - dot(fB, "Eliminate-09-fBL"); - fB = fB.combine(L, &add_); - dot(fB, "Eliminate-10-fB"); - printCounts("Eliminate L"); + // Eliminate L + ADT fB = fBL; + fB = apply(fB, fBL2, &mul); + dot(fB, "Eliminate-09-fBL"); + fB = fB.combine(L, &add_); + dot(fB, "Eliminate-10-fB"); + printCounts("Eliminate L"); } /* ************************************************************************* */ // test equality TEST(ADT, equality_noparser) { - DiscreteKey A(0,2), B(1,2); - Signature::Table tableA, tableB; - Signature::Row rA, rB; - rA += 80, 20; rB += 60, 40; - tableA += rA; tableB += rB; + DiscreteKey A(0,2), B(1,2); + Signature::Table tableA, tableB; + Signature::Row rA, rB; + rA += 80, 20; rB += 60, 40; + tableA += rA; tableB += rB; - // Check straight equality - ADT pA1 = create(A % tableA); - ADT pA2 = create(A % tableA); - EXPECT(pA1 == pA2); // should be equal + // Check straight equality + ADT pA1 = create(A % tableA); + ADT pA2 = create(A % tableA); + EXPECT(pA1 == pA2); // should be equal - // Check equality after apply - ADT pB = create(B % tableB); - ADT pAB1 = apply(pA1, pB, &mul); - ADT pAB2 = apply(pB, pA1, &mul); - EXPECT(pAB2 == pAB1); + // Check equality after apply + ADT pB = create(B % tableB); + ADT pAB1 = apply(pA1, pB, &mul); + ADT pAB2 = apply(pB, pA1, &mul); + EXPECT(pAB2 == pAB1); } /* ************************************************************************* */ // test equality TEST(ADT, equality_parser) { - DiscreteKey A(0,2), B(1,2); - // Check straight equality - ADT pA1 = create(A % "80/20"); - ADT pA2 = create(A % "80/20"); - EXPECT(pA1 == pA2); // should be equal + DiscreteKey A(0,2), B(1,2); + // Check straight equality + ADT pA1 = create(A % "80/20"); + ADT pA2 = create(A % "80/20"); + EXPECT(pA1 == pA2); // should be equal - // Check equality after apply - ADT pB = create(B % "60/40"); - ADT pAB1 = apply(pA1, pB, &mul); - ADT pAB2 = apply(pB, pA1, &mul); - EXPECT(pAB2 == pAB1); + // Check equality after apply + ADT pB = create(B % "60/40"); + ADT pAB1 = apply(pA1, pB, &mul); + ADT pAB2 = apply(pB, pA1, &mul); + EXPECT(pAB2 == pAB1); } /* ******************************************************************************** */ @@ -371,43 +371,43 @@ TEST(ADT, equality_parser) // test constructor from strings TEST(ADT, constructor) { - DiscreteKey v0(0,2), v1(1,3); - Assignment x00, x01, x02, x10, x11, x12; - x00[0] = 0, x00[1] = 0; - x01[0] = 0, x01[1] = 1; - x02[0] = 0, x02[1] = 2; - x10[0] = 1, x10[1] = 0; - x11[0] = 1, x11[1] = 1; - x12[0] = 1, x12[1] = 2; + DiscreteKey v0(0,2), v1(1,3); + Assignment x00, x01, x02, x10, x11, x12; + x00[0] = 0, x00[1] = 0; + x01[0] = 0, x01[1] = 1; + x02[0] = 0, x02[1] = 2; + x10[0] = 1, x10[1] = 0; + x11[0] = 1, x11[1] = 1; + x12[0] = 1, x12[1] = 2; - ADT f1(v0 & v1, "0 1 2 3 4 5"); - EXPECT_DOUBLES_EQUAL(0, f1(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(1, f1(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(2, f1(x02), 1e-9); - EXPECT_DOUBLES_EQUAL(3, f1(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(4, f1(x11), 1e-9); - EXPECT_DOUBLES_EQUAL(5, f1(x12), 1e-9); + ADT f1(v0 & v1, "0 1 2 3 4 5"); + EXPECT_DOUBLES_EQUAL(0, f1(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(1, f1(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(2, f1(x02), 1e-9); + EXPECT_DOUBLES_EQUAL(3, f1(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(4, f1(x11), 1e-9); + EXPECT_DOUBLES_EQUAL(5, f1(x12), 1e-9); - ADT f2(v1 & v0, "0 1 2 3 4 5"); - EXPECT_DOUBLES_EQUAL(0, f2(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(2, f2(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(4, f2(x02), 1e-9); - EXPECT_DOUBLES_EQUAL(1, f2(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(3, f2(x11), 1e-9); - EXPECT_DOUBLES_EQUAL(5, f2(x12), 1e-9); + ADT f2(v1 & v0, "0 1 2 3 4 5"); + EXPECT_DOUBLES_EQUAL(0, f2(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(2, f2(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(4, f2(x02), 1e-9); + EXPECT_DOUBLES_EQUAL(1, f2(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(3, f2(x11), 1e-9); + EXPECT_DOUBLES_EQUAL(5, f2(x12), 1e-9); - DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2); - vector table(5 * 4 * 3 * 2); - double x = 0; - BOOST_FOREACH(double& t, table) - t = x++; - ADT f3(z0 & z1 & z2 & z3, table); - Assignment assignment; - assignment[0] = 0; - assignment[1] = 0; - assignment[2] = 0; - assignment[3] = 1; - EXPECT_DOUBLES_EQUAL(1, f3(assignment), 1e-9); + DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2); + vector table(5 * 4 * 3 * 2); + double x = 0; + BOOST_FOREACH(double& t, table) + t = x++; + ADT f3(z0 & z1 & z2 & z3, table); + Assignment assignment; + assignment[0] = 0; + assignment[1] = 0; + assignment[2] = 0; + assignment[3] = 1; + EXPECT_DOUBLES_EQUAL(1, f3(assignment), 1e-9); } /* ************************************************************************* */ @@ -415,109 +415,109 @@ TEST(ADT, constructor) // Only works if DiscreteKeys are binary, as size_t has binary cardinality! TEST(ADT, conversion) { - DiscreteKey X(0,2), Y(1,2); - ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); - dot(fDiscreteKey, "conversion-f1"); + DiscreteKey X(0,2), Y(1,2); + ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); + dot(fDiscreteKey, "conversion-f1"); - std::map ordering; - ordering[0] = 5; - ordering[1] = 2; + std::map ordering; + ordering[0] = 5; + ordering[1] = 2; - AlgebraicDecisionTree fIndexKey(fDiscreteKey, ordering); - // f1.print("f1"); - // f2.print("f2"); - dot(fIndexKey, "conversion-f2"); + AlgebraicDecisionTree fIndexKey(fDiscreteKey, ordering); + // f1.print("f1"); + // f2.print("f2"); + dot(fIndexKey, "conversion-f2"); - Assignment x00, x01, x02, x10, x11, x12; - x00[5] = 0, x00[2] = 0; - x01[5] = 0, x01[2] = 1; - x10[5] = 1, x10[2] = 0; - x11[5] = 1, x11[2] = 1; - EXPECT_DOUBLES_EQUAL(0.2, fIndexKey(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(0.5, fIndexKey(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(0.3, fIndexKey(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(0.6, fIndexKey(x11), 1e-9); + Assignment x00, x01, x02, x10, x11, x12; + x00[5] = 0, x00[2] = 0; + x01[5] = 0, x01[2] = 1; + x10[5] = 1, x10[2] = 0; + x11[5] = 1, x11[2] = 1; + EXPECT_DOUBLES_EQUAL(0.2, fIndexKey(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, fIndexKey(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(0.3, fIndexKey(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(0.6, fIndexKey(x11), 1e-9); } /* ******************************************************************************** */ // test operations in elimination TEST(ADT, elimination) { - DiscreteKey A(0,2), B(1,3), C(2,2); - ADT f1(A & B & C, "1 2 3 4 5 6 1 8 3 3 5 5"); - dot(f1, "elimination-f1"); + DiscreteKey A(0,2), B(1,3), C(2,2); + ADT f1(A & B & C, "1 2 3 4 5 6 1 8 3 3 5 5"); + dot(f1, "elimination-f1"); - { - // sum out lower key - ADT actualSum = f1.sum(C); - ADT expectedSum(A & B, "3 7 11 9 6 10"); - CHECK(assert_equal(expectedSum,actualSum)); + { + // sum out lower key + ADT actualSum = f1.sum(C); + ADT expectedSum(A & B, "3 7 11 9 6 10"); + CHECK(assert_equal(expectedSum,actualSum)); - // normalize - ADT actual = f1 / actualSum; - vector cpt; - cpt += 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // - 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10; - ADT expected(A & B & C, cpt); - CHECK(assert_equal(expected,actual)); - } + // normalize + ADT actual = f1 / actualSum; + vector cpt; + cpt += 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // + 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10; + ADT expected(A & B & C, cpt); + CHECK(assert_equal(expected,actual)); + } - { - // sum out lower 2 keys - ADT actualSum = f1.sum(C).sum(B); - ADT expectedSum(A, 21, 25); - CHECK(assert_equal(expectedSum,actualSum)); + { + // sum out lower 2 keys + ADT actualSum = f1.sum(C).sum(B); + ADT expectedSum(A, 21, 25); + CHECK(assert_equal(expectedSum,actualSum)); - // normalize - ADT actual = f1 / actualSum; - vector cpt; - cpt += 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, // - 1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25; - ADT expected(A & B & C, cpt); - CHECK(assert_equal(expected,actual)); - } + // normalize + ADT actual = f1 / actualSum; + vector cpt; + cpt += 1.0 / 21, 2.0 / 21, 3.0 / 21, 4.0 / 21, 5.0 / 21, 6.0 / 21, // + 1.0 / 25, 8.0 / 25, 3.0 / 25, 3.0 / 25, 5.0 / 25, 5.0 / 25; + ADT expected(A & B & C, cpt); + CHECK(assert_equal(expected,actual)); + } } /* ******************************************************************************** */ // Test non-commutative op TEST(ADT, div) { - DiscreteKey A(0,2), B(1,2); + DiscreteKey A(0,2), B(1,2); - // Literals - ADT a(A, 8, 16); - ADT b(B, 2, 4); - ADT expected_a_div_b(A & B, "4 2 8 4"); // 8/2 8/4 16/2 16/4 - ADT expected_b_div_a(A & B, "0.25 0.5 0.125 0.25"); // 2/8 4/8 2/16 4/16 - EXPECT(assert_equal(expected_a_div_b, a / b)); - EXPECT(assert_equal(expected_b_div_a, b / a)); + // Literals + ADT a(A, 8, 16); + ADT b(B, 2, 4); + ADT expected_a_div_b(A & B, "4 2 8 4"); // 8/2 8/4 16/2 16/4 + ADT expected_b_div_a(A & B, "0.25 0.5 0.125 0.25"); // 2/8 4/8 2/16 4/16 + EXPECT(assert_equal(expected_a_div_b, a / b)); + EXPECT(assert_equal(expected_b_div_a, b / a)); } /* ******************************************************************************** */ // test zero shortcut TEST(ADT, zero) { - DiscreteKey A(0,2), B(1,2); + DiscreteKey A(0,2), B(1,2); - // Literals - ADT a(A, 0, 1); - ADT notb(B, 1, 0); - ADT anotb = a * notb; - // GTSAM_PRINT(anotb); - Assignment x00, x01, x10, x11; - x00[0] = 0, x00[1] = 0; - x01[0] = 0, x01[1] = 1; - x10[0] = 1, x10[1] = 0; - x11[0] = 1, x11[1] = 1; - EXPECT_DOUBLES_EQUAL(0, anotb(x00), 1e-9); - EXPECT_DOUBLES_EQUAL(0, anotb(x01), 1e-9); - EXPECT_DOUBLES_EQUAL(1, anotb(x10), 1e-9); - EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9); + // Literals + ADT a(A, 0, 1); + ADT notb(B, 1, 0); + ADT anotb = a * notb; + // GTSAM_PRINT(anotb); + Assignment x00, x01, x10, x11; + x00[0] = 0, x00[1] = 0; + x01[0] = 0, x01[1] = 1; + x10[0] = 1, x10[1] = 0; + x11[0] = 1, x11[1] = 1; + EXPECT_DOUBLES_EQUAL(0, anotb(x00), 1e-9); + EXPECT_DOUBLES_EQUAL(0, anotb(x01), 1e-9); + EXPECT_DOUBLES_EQUAL(1, anotb(x10), 1e-9); + EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9); } /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 1dfcd3057..1dfd56eec 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /* - * @file testDecisionTree.cpp - * @brief Develop DecisionTree - * @author Frank Dellaert - * @author Can Erdogan - * @date Jan 30, 2012 + * @file testDecisionTree.cpp + * @brief Develop DecisionTree + * @author Frank Dellaert + * @author Can Erdogan + * @date Jan 30, 2012 */ #include @@ -35,7 +35,7 @@ using namespace gtsam; template void dot(const T&f, const string& filename) { #ifndef DISABLE_DOT - f.dot(filename); + f.dot(filename); #endif } @@ -51,190 +51,190 @@ typedef DecisionTree CrazyDecisionTree; // check that DecisionTree typedef DecisionTree DT; struct Ring { - static inline int zero() { - return 0; - } - static inline int one() { - return 1; - } - static inline int add(const int& a, const int& b) { - return a + b; - } - static inline int mul(const int& a, const int& b) { - return a * b; - } + static inline int zero() { + return 0; + } + static inline int one() { + return 1; + } + static inline int add(const int& a, const int& b) { + return a + b; + } + static inline int mul(const int& a, const int& b) { + return a * b; + } }; /* ******************************************************************************** */ // test DT TEST(DT, example) { - // Create labels - string A("A"), B("B"), C("C"); + // Create labels + string A("A"), B("B"), C("C"); - // create a value - Assignment x00, x01, x10, x11; - x00[A] = 0, x00[B] = 0; - x01[A] = 0, x01[B] = 1; - x10[A] = 1, x10[B] = 0; - x11[A] = 1, x11[B] = 1; + // create a value + Assignment x00, x01, x10, x11; + x00[A] = 0, x00[B] = 0; + x01[A] = 0, x01[B] = 1; + x10[A] = 1, x10[B] = 0; + x11[A] = 1, x11[B] = 1; - // A - DT a(A, 0, 5); - LONGS_EQUAL(0,a(x00)) - LONGS_EQUAL(5,a(x10)) - DOT(a); + // A + DT a(A, 0, 5); + LONGS_EQUAL(0,a(x00)) + LONGS_EQUAL(5,a(x10)) + DOT(a); - // pruned - DT p(A, 2, 2); - LONGS_EQUAL(2,p(x00)) - LONGS_EQUAL(2,p(x10)) - DOT(p); + // pruned + DT p(A, 2, 2); + LONGS_EQUAL(2,p(x00)) + LONGS_EQUAL(2,p(x10)) + DOT(p); - // \neg B - DT notb(B, 5, 0); - LONGS_EQUAL(5,notb(x00)) - LONGS_EQUAL(5,notb(x10)) - DOT(notb); + // \neg B + DT notb(B, 5, 0); + LONGS_EQUAL(5,notb(x00)) + LONGS_EQUAL(5,notb(x10)) + DOT(notb); - // apply, two nodes, in natural order - DT anotb = apply(a, notb, &Ring::mul); - LONGS_EQUAL(0,anotb(x00)) - LONGS_EQUAL(0,anotb(x01)) - LONGS_EQUAL(25,anotb(x10)) - LONGS_EQUAL(0,anotb(x11)) - DOT(anotb); + // apply, two nodes, in natural order + DT anotb = apply(a, notb, &Ring::mul); + LONGS_EQUAL(0,anotb(x00)) + LONGS_EQUAL(0,anotb(x01)) + LONGS_EQUAL(25,anotb(x10)) + LONGS_EQUAL(0,anotb(x11)) + DOT(anotb); - // check pruning - DT pnotb = apply(p, notb, &Ring::mul); - LONGS_EQUAL(10,pnotb(x00)) - LONGS_EQUAL( 0,pnotb(x01)) - LONGS_EQUAL(10,pnotb(x10)) - LONGS_EQUAL( 0,pnotb(x11)) - DOT(pnotb); + // check pruning + DT pnotb = apply(p, notb, &Ring::mul); + LONGS_EQUAL(10,pnotb(x00)) + LONGS_EQUAL( 0,pnotb(x01)) + LONGS_EQUAL(10,pnotb(x10)) + LONGS_EQUAL( 0,pnotb(x11)) + DOT(pnotb); - // check pruning - DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); - LONGS_EQUAL(0,zeros(x00)) - LONGS_EQUAL(0,zeros(x01)) - LONGS_EQUAL(0,zeros(x10)) - LONGS_EQUAL(0,zeros(x11)) - DOT(zeros); + // check pruning + DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul); + LONGS_EQUAL(0,zeros(x00)) + LONGS_EQUAL(0,zeros(x01)) + LONGS_EQUAL(0,zeros(x10)) + LONGS_EQUAL(0,zeros(x11)) + DOT(zeros); - // apply, two nodes, in switched order - DT notba = apply(a, notb, &Ring::mul); - LONGS_EQUAL(0,notba(x00)) - LONGS_EQUAL(0,notba(x01)) - LONGS_EQUAL(25,notba(x10)) - LONGS_EQUAL(0,notba(x11)) - DOT(notba); + // apply, two nodes, in switched order + DT notba = apply(a, notb, &Ring::mul); + LONGS_EQUAL(0,notba(x00)) + LONGS_EQUAL(0,notba(x01)) + LONGS_EQUAL(25,notba(x10)) + LONGS_EQUAL(0,notba(x11)) + DOT(notba); - // Test choose 0 - DT actual0 = notba.choose(A, 0); - EXPECT(assert_equal(DT(0.0), actual0)); - DOT(actual0); + // Test choose 0 + DT actual0 = notba.choose(A, 0); + EXPECT(assert_equal(DT(0.0), actual0)); + DOT(actual0); - // Test choose 1 - DT actual1 = notba.choose(A, 1); - EXPECT(assert_equal(DT(B, 25, 0), actual1)); - DOT(actual1); + // Test choose 1 + DT actual1 = notba.choose(A, 1); + EXPECT(assert_equal(DT(B, 25, 0), actual1)); + DOT(actual1); - // apply, two nodes at same level - DT a_and_a = apply(a, a, &Ring::mul); - LONGS_EQUAL(0,a_and_a(x00)) - LONGS_EQUAL(0,a_and_a(x01)) - LONGS_EQUAL(25,a_and_a(x10)) - LONGS_EQUAL(25,a_and_a(x11)) - DOT(a_and_a); + // apply, two nodes at same level + DT a_and_a = apply(a, a, &Ring::mul); + LONGS_EQUAL(0,a_and_a(x00)) + LONGS_EQUAL(0,a_and_a(x01)) + LONGS_EQUAL(25,a_and_a(x10)) + LONGS_EQUAL(25,a_and_a(x11)) + DOT(a_and_a); - // create a function on C - DT c(C, 0, 5); + // create a function on C + DT c(C, 0, 5); - // and a model assigning stuff to C - Assignment x101; - x101[A] = 1, x101[B] = 0, x101[C] = 1; + // and a model assigning stuff to C + Assignment x101; + x101[A] = 1, x101[B] = 0, x101[C] = 1; - // mul notba with C - DT notbac = apply(notba, c, &Ring::mul); - LONGS_EQUAL(125,notbac(x101)) - DOT(notbac); + // mul notba with C + DT notbac = apply(notba, c, &Ring::mul); + LONGS_EQUAL(125,notbac(x101)) + DOT(notbac); - // mul now in different order - DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); - LONGS_EQUAL(125,acnotb(x101)) - DOT(acnotb); + // mul now in different order + DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul); + LONGS_EQUAL(125,acnotb(x101)) + DOT(acnotb); } /* ******************************************************************************** */ // test Conversion enum Label { - U, V, X, Y, Z + U, V, X, Y, Z }; typedef DecisionTree BDT; bool convert(const int& y) { - return y != 0; + return y != 0; } TEST(DT, conversion) { - // Create labels - string A("A"), B("B"); + // Create labels + string A("A"), B("B"); - // apply, two nodes, in natural order - DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul); + // apply, two nodes, in natural order + DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul); - // convert - map ordering; - ordering[A] = X; - ordering[B] = Y; - boost::function op = convert; - BDT f2(f1, ordering, op); - // f1.print("f1"); - // f2.print("f2"); + // convert + map ordering; + ordering[A] = X; + ordering[B] = Y; + boost::function op = convert; + BDT f2(f1, ordering, op); + // f1.print("f1"); + // f2.print("f2"); - // create a value - Assignment