changed tabs to spaces for consistent indentation in all of GTSAM
Chris Beall
parent
96ce28625b
commit
4297d24c96
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -18,58 +18,58 @@
|
|||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
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<std::string> (__FILE__),
|
||||
__LINE__,
|
||||
boost::lexical_cast<std::string> (expected),
|
||||
boost::lexical_cast<std::string> (actual)));
|
||||
if (expected == actual)
|
||||
return true;
|
||||
result.addFailure (
|
||||
Failure (
|
||||
name_,
|
||||
boost::lexical_cast<std::string> (__FILE__),
|
||||
__LINE__,
|
||||
boost::lexical_cast<std::string> (expected),
|
||||
boost::lexical_cast<std::string> (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<std::string> (__FILE__),
|
||||
__LINE__,
|
||||
expected,
|
||||
actual));
|
||||
if (expected == actual)
|
||||
return true;
|
||||
result.addFailure (
|
||||
Failure (
|
||||
name_,
|
||||
boost::lexical_cast<std::string> (__FILE__),
|
||||
__LINE__,
|
||||
expected,
|
||||
actual));
|
||||
|
||||
return false;
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<std::string>(#condition))); \
|
||||
return; } \
|
||||
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#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<std::string>(#condition))); \
|
||||
return; } \
|
||||
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
|
||||
return; } \
|
||||
catch (exception_name&) {} \
|
||||
catch (...) { \
|
||||
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast<std::string>(#condition) + boost::lexical_cast<std::string>(", expected: ") + boost::lexical_cast<std::string>(#exception_name))); \
|
||||
return; } }
|
||||
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast<std::string>(#condition) + boost::lexical_cast<std::string>(", expected: ") + boost::lexical_cast<std::string>(#exception_name))); \
|
||||
return; } }
|
||||
|
||||
#define EQUALITY(expected,actual)\
|
||||
{ if (!assert_equal(expected,actual)) \
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/nonlinear/Symbol.h>
|
||||
|
|
@ -30,27 +30,27 @@ using symbol_shorthand::X;
|
|||
* a known 3D point in the image
|
||||
*/
|
||||
class ResectioningFactor: public NoiseModelFactor1<Pose3> {
|
||||
typedef NoiseModelFactor1<Pose3> Base;
|
||||
typedef NoiseModelFactor1<Pose3> 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<Matrix&> 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<Matrix&> 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<ResectioningFactor> factor;
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
Point2(55, 45), Point3(10, 10, 0)));
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
Point2(45, 45), Point3(-10, 10, 0)));
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
Point2(45, 55), Point3(-10, -10, 0)));
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(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<ResectioningFactor> factor;
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
Point2(55, 45), Point3(10, 10, 0)));
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
Point2(45, 45), Point3(-10, 10, 0)));
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
|
||||
Point2(45, 55), Point3(-10, -10, 0)));
|
||||
graph.push_back(
|
||||
boost::make_shared<ResectioningFactor>(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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<Pose2>(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<Pose2>(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<Pose2>(x1, x2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(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<Pose2>(x1, x2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(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<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
|
||||
// Add Bearing-Range factors
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Pose2>(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<Pose2>(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<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
|
||||
// 2b. Add odometry factors
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(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<Pose2>(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<Pose2>(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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<Pose2>(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<Pose2>(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;
|
||||
|
|
|
|||
|
|
@ -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<Rot2> 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<Rot2> 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<DiscreteKey> 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<DiscreteKey>::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<DiscreteKey>::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<DiscreteKey>::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<DiscreteKey>::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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<DiscreteFactor::Values> 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<DiscreteFactor::Values> 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -514,12 +514,12 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
|
|||
typedef typename LDLTType::RealScalar RealScalar;
|
||||
const Diagonal<const MatrixType> vectorD = dec().vectorD();
|
||||
RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
|
||||
RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
|
||||
RealScalar(1) / NumTraits<RealScalar>::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)
|
||||
|
|
|
|||
|
|
@ -324,7 +324,7 @@ void ComplexEigenSolver<MatrixType>::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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -547,7 +547,7 @@ void EigenSolver<MatrixType>::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<Scalar> cc = cdiv(x*lastra-lastw*ra+q*sa,x*lastsa-lastw*sa-q*ra,vr,vi);
|
||||
std::complex<Scalar> 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)))
|
||||
|
|
|
|||
|
|
@ -197,8 +197,8 @@ template<typename _MatrixType> 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
|
||||
|
|
|
|||
|
|
@ -141,10 +141,10 @@ MatrixBase<Derived>::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<Lower>()
|
||||
.eigenvalues()
|
||||
.maxCoeff()
|
||||
);
|
||||
.template selfadjointView<Lower>()
|
||||
.eigenvalues()
|
||||
.maxCoeff()
|
||||
);
|
||||
}
|
||||
|
||||
/** \brief Computes the L2 operator norm
|
||||
|
|
|
|||
|
|
@ -200,8 +200,8 @@ template<typename _MatrixType> 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
|
||||
|
|
|
|||
|
|
@ -179,7 +179,7 @@ public:
|
|||
bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::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
|
||||
|
|
|
|||
|
|
@ -487,7 +487,7 @@ struct solve_retval<ColPivHouseholderQR<_MatrixType>, 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()
|
||||
|
|
|
|||
|
|
@ -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 *, \
|
||||
|
|
|
|||
|
|
@ -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 *);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_
|
||||
|
||||
|
|
|
|||
|
|
@ -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 *);
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
Matrix<int, 3, 4, ColMajor> 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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -68,8 +68,8 @@ template<typename MatrixType, int CRows, int CCols, int SRows, int SCols> void c
|
|||
cols = MatrixType::ColsAtCompileTime,
|
||||
r = CRows,
|
||||
c = CCols,
|
||||
sr = SRows,
|
||||
sc = SCols
|
||||
sr = SRows,
|
||||
sc = SCols
|
||||
};
|
||||
|
||||
VERIFY_IS_EQUAL((matrix.template topLeftCorner<r,c>()), (matrix.template block<r,c>(0,0)));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ template<typename Scalar,int Size> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ template<typename MatrixType> 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<ComplexScalar>(), U * T * U.adjoint());
|
||||
|
|
|
|||
|
|
@ -69,74 +69,74 @@ namespace internal {
|
|||
*/
|
||||
template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -248,7 +248,7 @@ template <typename MatrixType>
|
|||
EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::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 <typename MatrixType>
|
|||
EIGEN_STRONG_INLINE void MatrixExponential<MatrixType>::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;
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ namespace Eigen {
|
|||
* \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic
|
||||
*/
|
||||
template <typename MatrixType,
|
||||
typename AtomicType,
|
||||
typename AtomicType,
|
||||
int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
|
||||
class MatrixFunction
|
||||
{
|
||||
|
|
@ -267,13 +267,13 @@ void MatrixFunction<MatrixType,AtomicType,1>::partitionEigenvalues()
|
|||
// Look for other element to add to the set
|
||||
for (Index j=i+1; j<rows; ++j) {
|
||||
if (internal::abs(diag(j) - diag(i)) <= separation() && std::find(qi->begin(), 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<MatrixType,AtomicType,1>::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<MatrixType,AtomicType,1>::DynMatrixType MatrixFunction<M
|
|||
// Compute AX = \sum_{k=i+1}^m A_{ik} X_{kj}
|
||||
Scalar AX;
|
||||
if (i == m - 1) {
|
||||
AX = 0;
|
||||
AX = 0;
|
||||
} else {
|
||||
Matrix<Scalar,1,1> AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i);
|
||||
AX = AXmatrix(0,0);
|
||||
Matrix<Scalar,1,1> 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<Scalar,1,1> XBmatrix = X.row(i).head(j) * B.col(j).head(j);
|
||||
XB = XBmatrix(0,0);
|
||||
Matrix<Scalar,1,1> 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));
|
||||
|
|
|
|||
|
|
@ -118,7 +118,7 @@ void MatrixFunctionAtomic<MatrixType>::computeMu()
|
|||
/** \brief Determine whether Taylor series has converged */
|
||||
template <typename MatrixType>
|
||||
bool MatrixFunctionAtomic<MatrixType>::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();
|
||||
|
|
|
|||
|
|
@ -168,7 +168,7 @@ void MatrixLogarithmAtomic<MatrixType>::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<MatrixType>::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());
|
||||
|
|
|
|||
|
|
@ -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 <typename SmallMatrixType>
|
||||
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<MatrixType>::compute(ResultType &result)
|
|||
// post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
|
||||
template <typename MatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>::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<MatrixType>::computeDiagonalPartOfSqrt(Matr
|
|||
// post: sqrtT is the square root of T.
|
||||
template <typename MatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>::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<MatrixType>
|
|||
template <typename MatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>
|
||||
::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<MatrixType>
|
|||
template <typename MatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>
|
||||
::compute1x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
|
||||
typename MatrixType::Index i, typename MatrixType::Index j)
|
||||
typename MatrixType::Index i, typename MatrixType::Index j)
|
||||
{
|
||||
Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
|
||||
if (j-i > 1)
|
||||
|
|
@ -198,7 +198,7 @@ void MatrixSquareRootQuasiTriangular<MatrixType>
|
|||
template <typename MatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>
|
||||
::compute2x1offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
|
||||
typename MatrixType::Index i, typename MatrixType::Index j)
|
||||
typename MatrixType::Index i, typename MatrixType::Index j)
|
||||
{
|
||||
Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
|
||||
if (j-i > 2)
|
||||
|
|
@ -212,7 +212,7 @@ void MatrixSquareRootQuasiTriangular<MatrixType>
|
|||
template <typename MatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>
|
||||
::compute2x2offDiagonalBlock(MatrixType& sqrtT, const MatrixType& T,
|
||||
typename MatrixType::Index i, typename MatrixType::Index j)
|
||||
typename MatrixType::Index i, typename MatrixType::Index j)
|
||||
{
|
||||
Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
|
||||
Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
|
||||
|
|
@ -229,10 +229,10 @@ template <typename MatrixType>
|
|||
template <typename SmallMatrixType>
|
||||
void MatrixSquareRootQuasiTriangular<MatrixType>
|
||||
::solveAuxiliaryEquation(SmallMatrixType& X, const SmallMatrixType& A,
|
||||
const SmallMatrixType& B, const SmallMatrixType& C)
|
||||
const SmallMatrixType& B, const SmallMatrixType& C)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT((internal::is_same<SmallMatrixType, Matrix<Scalar,2,2> >::value),
|
||||
EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT);
|
||||
EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT);
|
||||
|
||||
Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero();
|
||||
coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -253,9 +253,9 @@ bool saveMarket(const SparseMatrixType& mat, const std::string& filename, int sy
|
|||
for(int j=0; j<mat.outerSize(); ++j)
|
||||
for(typename SparseMatrixType::InnerIterator it(mat,j); it; ++it)
|
||||
{
|
||||
++ count;
|
||||
internal::PutMatrixElt(it.value(), it.row()+1, it.col()+1, out);
|
||||
// out << it.row()+1 << " " << it.col()+1 << " " << it.value() << "\n";
|
||||
++ count;
|
||||
internal::PutMatrixElt(it.value(), it.row()+1, it.col()+1, out);
|
||||
// out << it.row()+1 << " " << it.col()+1 << " " << it.value() << "\n";
|
||||
}
|
||||
out.close();
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ struct generateTestMatrix<MatrixType,0>
|
|||
typename EigenSolver<MatrixType>::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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <cstring>
|
||||
#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;i<n;i++) t[i] = (mpfr_ptr)tab[i].mp;
|
||||
mpfr_sum(x.mp,t,n,rnd_mode);
|
||||
delete[] t;
|
||||
return x;
|
||||
t = new mpfr_ptr[n];
|
||||
for (i=0;i<n;i++) t[i] = (mpfr_ptr)tab[i].mp;
|
||||
mpfr_sum(x.mp,t,n,rnd_mode);
|
||||
delete[] t;
|
||||
return x;
|
||||
}
|
||||
|
||||
const mpreal remquo (long* q, 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_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 <class T>
|
||||
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<size_t>(exp)<slen)
|
||||
{
|
||||
if(s[0]=='-')
|
||||
{
|
||||
// Remove zeros starting from right end
|
||||
char* ptr = s+slen-1;
|
||||
while (*ptr=='0' && ptr>s+exp) ptr--;
|
||||
// Make human eye-friendly formatting if possible
|
||||
if (exp>0 && static_cast<size_t>(exp)<slen)
|
||||
{
|
||||
if(s[0]=='-')
|
||||
{
|
||||
// Remove zeros starting from right end
|
||||
char* ptr = s+slen-1;
|
||||
while (*ptr=='0' && ptr>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<mp_exp_t>(exp,std::dec);
|
||||
else out += "e"+mpfr::toString<mp_exp_t>(exp,std::dec);
|
||||
}
|
||||
}
|
||||
// Make final string
|
||||
if(--exp)
|
||||
{
|
||||
if(exp>0) out += "e+"+mpfr::toString<mp_exp_t>(exp,std::dec);
|
||||
else out += "e"+mpfr::toString<mp_exp_t>(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<<v.toString(static_cast<int>(os.precision()));
|
||||
return os<<v.toString(static_cast<int>(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
|
||||
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -26,71 +26,71 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
DSFVector::DSFVector (const size_t numNodes) {
|
||||
v_ = boost::make_shared<V>(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<V>(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>& v_in, const std::vector<size_t>& 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<size_t> DSFVector::set(const Label& label) const {
|
||||
std::set<size_t> set;
|
||||
V::const_iterator it = keys_.begin();
|
||||
for (; it != keys_.end(); it++) {
|
||||
if (findSet(*it) == label)
|
||||
set.insert(*it);
|
||||
}
|
||||
return set;
|
||||
std::set<size_t> set;
|
||||
V::const_iterator it = keys_.begin();
|
||||
for (; it != keys_.end(); it++) {
|
||||
if (findSet(*it) == label)
|
||||
set.insert(*it);
|
||||
}
|
||||
return set;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::map<DSFVector::Label, std::set<size_t> > DSFVector::sets() const {
|
||||
std::map<Label, std::set<size_t> > sets;
|
||||
V::const_iterator it = keys_.begin();
|
||||
for (; it != keys_.end(); it++) {
|
||||
sets[findSet(*it)].insert(*it);
|
||||
}
|
||||
return sets;
|
||||
std::map<Label, std::set<size_t> > sets;
|
||||
V::const_iterator it = keys_.begin();
|
||||
for (; it != keys_.end(); it++) {
|
||||
sets[findSet(*it)].insert(*it);
|
||||
}
|
||||
return sets;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::map<DSFVector::Label, std::vector<size_t> > DSFVector::arrays() const {
|
||||
std::map<Label, std::vector<size_t> > arrays;
|
||||
V::const_iterator it = keys_.begin();
|
||||
for (; it != keys_.end(); it++) {
|
||||
arrays[findSet(*it)].push_back(*it);
|
||||
}
|
||||
return arrays;
|
||||
std::map<Label, std::vector<size_t> > 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
|
||||
|
|
|
|||
|
|
@ -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<size_t> 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<size_t> 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> v_;
|
||||
std::vector<size_t> keys_;
|
||||
private:
|
||||
// TODO could use existing memory to improve the efficiency
|
||||
boost::shared_ptr<V> v_;
|
||||
std::vector<size_t> 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>& v_in, const std::vector<size_t>& keys);
|
||||
/// constructor that uses the existing memory
|
||||
DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& 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<size_t> set(const Label& label) const;
|
||||
/// get the nodes in the tree with the given label
|
||||
std::set<size_t> set(const Label& label) const;
|
||||
|
||||
/// return all sets, i.e. a partition of all elements
|
||||
std::map<Label, std::set<size_t> > sets() const;
|
||||
/// return all sets, i.e. a partition of all elements
|
||||
std::map<Label, std::set<size_t> > sets() const;
|
||||
|
||||
/// return all sets, i.e. a partition of all elements
|
||||
std::map<Label, std::vector<size_t> > arrays() const;
|
||||
/// return all sets, i.e. a partition of all elements
|
||||
std::map<Label, std::vector<size_t> > 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);
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,21 +26,21 @@ template<class DERIVED>
|
|||
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<PoolTag, sizeof(DERIVED)>::malloc();
|
||||
DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
|
||||
return ptr;
|
||||
void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
|
||||
DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
|
||||
return ptr;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -48,15 +48,15 @@ public:
|
|||
*/
|
||||
virtual void deallocate_() const {
|
||||
this->~DerivedValue(); // Virtual destructor cleans up the derived object
|
||||
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this); // Release memory from pool
|
||||
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this); // Release memory from pool
|
||||
}
|
||||
|
||||
/**
|
||||
* Clone this value (normal clone on the heap, delete with 'delete' operator)
|
||||
*/
|
||||
virtual boost::shared_ptr<Value> clone() const {
|
||||
return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
|
||||
}
|
||||
/**
|
||||
* Clone this value (normal clone on the heap, delete with 'delete' operator)
|
||||
*/
|
||||
virtual boost::shared_ptr<Value> clone() const {
|
||||
return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*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<const DERIVED*>(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<const DERIVED&>(value2);
|
||||
|
||||
// Return the result of calling localCoordinates on the derived class
|
||||
return (static_cast<const DERIVED*>(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<const DERIVED&>(rhs);
|
||||
|
||||
// Do the assignment and return the result
|
||||
return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
|
||||
}
|
||||
}
|
||||
|
||||
/// Conversion to the derived class
|
||||
operator const DERIVED& () const {
|
||||
return static_cast<const DERIVED&>(*this);
|
||||
}
|
||||
/// Conversion to the derived class
|
||||
operator const DERIVED& () const {
|
||||
return static_cast<const DERIVED&>(*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<DERIVED>& operator=(const DerivedValue<DERIVED>& 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<DERIVED>& operator=(const DerivedValue<DERIVED>& 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 { };
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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<typename VALUE>
|
||||
class FastList: public std::list<VALUE, boost::fast_pool_allocator<VALUE> > {
|
||||
|
|
|
|||
|
|
@ -88,10 +88,10 @@ public:
|
|||
Base::assign(x.begin(), x.end());
|
||||
}
|
||||
|
||||
/** Conversion to a standard STL container */
|
||||
operator std::vector<VALUE>() const {
|
||||
return std::vector<VALUE>(this->begin(), this->end());
|
||||
}
|
||||
/** Conversion to a standard STL container */
|
||||
operator std::vector<VALUE>() const {
|
||||
return std::vector<VALUE>(this->begin(), this->end());
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -28,21 +28,21 @@ namespace gtsam {
|
|||
template<class T>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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 T>
|
||||
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<class T>
|
||||
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 <class T> Matrix wedge(const Vector& x);
|
|||
*/
|
||||
template <class T>
|
||||
T expm(const Vector& x, int K=7) {
|
||||
Matrix xhat = wedge<T>(x);
|
||||
return T(expm(xhat,K));
|
||||
Matrix xhat = wedge<T>(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<T>; \
|
||||
template class gtsam::GroupConcept<T>; \
|
||||
template class gtsam::LieConcept<T>;
|
||||
template class gtsam::ManifoldConcept<T>; \
|
||||
template class gtsam::GroupConcept<T>; \
|
||||
template class gtsam::LieConcept<T>;
|
||||
|
||||
#define GTSAM_CONCEPT_LIE_TYPE(T) \
|
||||
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
|
||||
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
|
||||
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
|
||||
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
|
||||
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
|
||||
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
|
||||
|
|
|
|||
|
|
@ -31,109 +31,109 @@ namespace gtsam {
|
|||
*/
|
||||
struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
|
||||
|
||||
/** 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<int M, int N>
|
||||
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
|
||||
/** initialize from a fixed size normal vector */
|
||||
template<int M, int N>
|
||||
LieMatrix(const Eigen::Matrix<double, M, N>& 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<const Matrix>(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<Matrix>(*this);
|
||||
}
|
||||
/** get the underlying vector */
|
||||
inline Matrix matrix() const {
|
||||
return static_cast<Matrix>(*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<const Matrix>(&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<Matrix>(&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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H=boost::none) const {
|
||||
if(H) *H = -eye(dim());
|
||||
/** invert the object and yield a new one */
|
||||
inline LieMatrix inverse(boost::optional<gtsam::Matrix&> 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<const Vector>(&p(0,0), p.dim()); }
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -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> {
|
||||
/**
|
||||
* LieScalar is a wrapper around double to allow it to be a Lie type
|
||||
*/
|
||||
struct LieScalar : public DerivedValue<LieScalar> {
|
||||
|
||||
/** 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<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> 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
|
||||
|
|
|
|||
|
|
@ -28,96 +28,96 @@ namespace gtsam {
|
|||
*/
|
||||
struct LieVector : public Vector, public DerivedValue<LieVector> {
|
||||
|
||||
/** 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<int N>
|
||||
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
|
||||
/** initialize from a fixed size normal vector */
|
||||
template<int N>
|
||||
LieVector(const Eigen::Matrix<double, N, 1>& 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<Vector>(*this);
|
||||
}
|
||||
/** get the underlying vector */
|
||||
inline Vector vector() const {
|
||||
return static_cast<Vector>(*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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> 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<gtsam::Matrix&> H=boost::none) const {
|
||||
if(H) *H = -eye(dim());
|
||||
/** invert the object and yield a new one */
|
||||
inline LieVector inverse(boost::optional<gtsam::Matrix&> 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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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 T>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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<Matrix>& As, const std::list<Matrix>& Bs, double tol) {
|
||||
if (As.size() != Bs.size()) return false;
|
||||
if (As.size() != Bs.size()) return false;
|
||||
|
||||
list<Matrix>::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<Matrix>::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<m1; i++) {
|
||||
if (!gtsam::linear_dependent(Vector(row(A,i)), Vector(row(B,i)), tol))
|
||||
dependent = false;
|
||||
}
|
||||
for(size_t i=0; dependent && i<m1; i++) {
|
||||
if (!gtsam::linear_dependent(Vector(row(A,i)), Vector(row(B,i)), tol))
|
||||
dependent = false;
|
||||
}
|
||||
|
||||
return dependent;
|
||||
return dependent;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool linear_independent(const Matrix& A, const Matrix& B, double tol) {
|
||||
if(!is_linear_dependent(A, B, tol))
|
||||
return true;
|
||||
else {
|
||||
cout << "not linearly dependent:" << endl;
|
||||
print(A,"A = ");
|
||||
print(B,"B = ");
|
||||
if(A.rows()!=B.rows() || A.cols()!=B.cols())
|
||||
cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
|
||||
return false;
|
||||
}
|
||||
if(!is_linear_dependent(A, B, tol))
|
||||
return true;
|
||||
else {
|
||||
cout << "not linearly dependent:" << endl;
|
||||
print(A,"A = ");
|
||||
print(B,"B = ");
|
||||
if(A.rows()!=B.rows() || A.cols()!=B.cols())
|
||||
cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
|
||||
if(is_linear_dependent(A, B, tol))
|
||||
return true;
|
||||
else {
|
||||
cout << "not linearly dependent:" << endl;
|
||||
print(A,"A = ");
|
||||
print(B,"B = ");
|
||||
if(A.rows()!=B.rows() || A.cols()!=B.cols())
|
||||
cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
|
||||
return false;
|
||||
}
|
||||
if(is_linear_dependent(A, B, tol))
|
||||
return true;
|
||||
else {
|
||||
cout << "not linearly dependent:" << endl;
|
||||
print(A,"A = ");
|
||||
print(B,"B = ");
|
||||
if(A.rows()!=B.rows() || A.cols()!=B.cols())
|
||||
cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) {
|
||||
e += alpha * A * x;
|
||||
e += alpha * A * x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) {
|
||||
e += A * x;
|
||||
e += A * x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector operator^(const Matrix& A, const Vector & v) {
|
||||
if (A.rows()!=v.size()) throw std::invalid_argument(
|
||||
boost::str(boost::format("Matrix operator^ : A.m(%d)!=v.size(%d)") % A.rows() % v.size()));
|
||||
// Vector vt = v.transpose();
|
||||
// Vector vtA = vt * A;
|
||||
// return vtA.transpose();
|
||||
return A.transpose() * v;
|
||||
if (A.rows()!=v.size()) throw std::invalid_argument(
|
||||
boost::str(boost::format("Matrix operator^ : A.m(%d)!=v.size(%d)") % A.rows() % v.size()));
|
||||
// Vector vt = v.transpose();
|
||||
// Vector vtA = vt * A;
|
||||
// return vtA.transpose();
|
||||
return A.transpose() * v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) {
|
||||
x += alpha * A.transpose() * e;
|
||||
x += alpha * A.transpose() * e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) {
|
||||
x += A.transpose() * e;
|
||||
x += A.transpose() * e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
|
||||
x += alpha * A.transpose() * e;
|
||||
x += alpha * A.transpose() * e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Vector_(const Matrix& A)
|
||||
{
|
||||
size_t m = A.rows(), n = A.cols();
|
||||
Vector v(m*n);
|
||||
for( size_t j = 0, k=0 ; j < n ; j++)
|
||||
for( size_t i = 0; i < m ; i++,k++)
|
||||
v(k) = A(i,j);
|
||||
return v;
|
||||
size_t m = A.rows(), n = A.cols();
|
||||
Vector v(m*n);
|
||||
for( size_t j = 0, k=0 ; j < n ; j++)
|
||||
for( size_t i = 0; i < m ; i++,k++)
|
||||
v(k) = A(i,j);
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void print(const Matrix& A, const string &s, ostream& stream) {
|
||||
size_t m = A.rows(), n = A.cols();
|
||||
size_t m = A.rows(), n = A.cols();
|
||||
|
||||
// print out all elements
|
||||
stream << s << "[\n";
|
||||
for( size_t i = 0 ; i < m ; i++) {
|
||||
for( size_t j = 0 ; j < n ; j++) {
|
||||
double aij = A(i,j);
|
||||
if(aij != 0.0)
|
||||
stream << setw(12) << setprecision(9) << aij << ",\t";
|
||||
else
|
||||
stream << " 0.0,\t";
|
||||
}
|
||||
stream << endl;
|
||||
}
|
||||
stream << "];" << endl;
|
||||
// print out all elements
|
||||
stream << s << "[\n";
|
||||
for( size_t i = 0 ; i < m ; i++) {
|
||||
for( size_t j = 0 ; j < n ; j++) {
|
||||
double aij = A(i,j);
|
||||
if(aij != 0.0)
|
||||
stream << setw(12) << setprecision(9) << aij << ",\t";
|
||||
else
|
||||
stream << " 0.0,\t";
|
||||
}
|
||||
stream << endl;
|
||||
}
|
||||
stream << "];" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void save(const Matrix& A, const string &s, const string& filename) {
|
||||
fstream stream(filename.c_str(), fstream::out | fstream::app);
|
||||
print(A, s + "=", stream);
|
||||
stream.close();
|
||||
fstream stream(filename.c_str(), fstream::out | fstream::app);
|
||||
print(A, s + "=", stream);
|
||||
stream.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -271,126 +271,126 @@ istream& operator>>(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<Matrix,Matrix> 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<j ? 0.0 : vjm(k-j);
|
||||
// pad with zeros to get m-dimensional vector v
|
||||
for(size_t k = 0 ; k < m; k++)
|
||||
v(k) = k<j ? 0.0 : vjm(k-j);
|
||||
|
||||
// create Householder reflection matrix Qj = I-beta*v*v'
|
||||
Matrix Qj = eye(m) - beta * v * v.transpose();
|
||||
// create Householder reflection matrix Qj = I-beta*v*v'
|
||||
Matrix Qj = eye(m) - beta * v * v.transpose();
|
||||
|
||||
R = Qj * R; // update R
|
||||
Q = Q * Qj; // update Q
|
||||
R = Qj * R; // update R
|
||||
Q = Q * Qj; // update Q
|
||||
|
||||
} // column j
|
||||
} // column j
|
||||
|
||||
return make_pair(Q,R);
|
||||
return make_pair(Q,R);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<boost::tuple<Vector, double, double> >
|
||||
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<boost::tuple<Vector, double, double> > results;
|
||||
// create list
|
||||
list<boost::tuple<Vector, double, double> > 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<n; ++j) {
|
||||
// extract the first column of A
|
||||
Vector a(column(A, j));
|
||||
// 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<n; ++j) {
|
||||
// extract the first column of A
|
||||
Vector a(column(A, j));
|
||||
|
||||
// Calculate weighted pseudo-inverse and corresponding precision
|
||||
double precision = weightedPseudoinverse(a, weights, pseudo);
|
||||
// Calculate weighted pseudo-inverse and corresponding precision
|
||||
double precision = weightedPseudoinverse(a, weights, pseudo);
|
||||
|
||||
// if precision is zero, no information on this column
|
||||
if (precision < 1e-8) continue;
|
||||
// if precision is zero, no information on this column
|
||||
if (precision < 1e-8) continue;
|
||||
|
||||
// create solution and copy into r
|
||||
Vector r(basis(n, j));
|
||||
for (size_t j2=j+1; j2<n; ++j2)
|
||||
r(j2) = pseudo.dot(A.col(j2));
|
||||
// create solution and copy into r
|
||||
Vector r(basis(n, j));
|
||||
for (size_t j2=j+1; j2<n; ++j2)
|
||||
r(j2) = pseudo.dot(A.col(j2));
|
||||
|
||||
// create the rhs
|
||||
double d = pseudo.dot(b);
|
||||
// create the rhs
|
||||
double d = pseudo.dot(b);
|
||||
|
||||
// construct solution (r, d, sigma)
|
||||
// TODO: avoid sqrt, store precision or at least variance
|
||||
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
|
||||
// construct solution (r, d, sigma)
|
||||
// TODO: avoid sqrt, store precision or at least variance
|
||||
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
|
||||
|
||||
// exit after rank exhausted
|
||||
if (results.size()>=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<Eigen::UnitLower>().solve(b);
|
||||
else
|
||||
return L.triangularView<Eigen::Lower>().solve(b);
|
||||
// @return the solution x of L*x=b
|
||||
assert(L.rows() == L.cols());
|
||||
if (unit)
|
||||
return L.triangularView<Eigen::UnitLower>().solve(b);
|
||||
else
|
||||
return L.triangularView<Eigen::Lower>().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<Eigen::UnitUpper>().solve(b);
|
||||
else
|
||||
return U.triangularView<Eigen::Upper>().solve(b);
|
||||
// @return the solution x of U*x=b
|
||||
assert(U.rows() == U.cols());
|
||||
if (unit)
|
||||
return U.triangularView<Eigen::UnitUpper>().solve(b);
|
||||
else
|
||||
return U.triangularView<Eigen::Upper>().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<Eigen::UnitUpper>().transpose().solve<Eigen::OnTheLeft>(b);
|
||||
else
|
||||
return U.triangularView<Eigen::Upper>().transpose().solve<Eigen::OnTheLeft>(b);
|
||||
// @return the solution x of x'*U=b'
|
||||
assert(U.rows() == U.cols());
|
||||
if (unit)
|
||||
return U.triangularView<Eigen::UnitUpper>().transpose().solve<Eigen::OnTheLeft>(b);
|
||||
else
|
||||
return U.triangularView<Eigen::Upper>().transpose().solve<Eigen::OnTheLeft>(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<const Matrix *>& 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<const Matrix *> 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<const Matrix *> 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<m; ++i) {
|
||||
const double& vi = v(i);
|
||||
if (!isnan(vi) && !isinf(vi))
|
||||
A.row(i) *= vi;
|
||||
}
|
||||
} else {
|
||||
for (size_t i=0; i<m; ++i)
|
||||
A.row(i) *= v(i);
|
||||
}
|
||||
const size_t m = A.rows();
|
||||
if (inf_mask) {
|
||||
for (size_t i=0; i<m; ++i) {
|
||||
const double& vi = v(i);
|
||||
if (!isnan(vi) && !isinf(vi))
|
||||
A.row(i) *= vi;
|
||||
}
|
||||
} else {
|
||||
for (size_t i=0; i<m; ++i)
|
||||
A.row(i) *= v(i);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// row scaling
|
||||
Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask) {
|
||||
Matrix M(A);
|
||||
vector_scale_inplace(v, M, inf_mask);
|
||||
return M;
|
||||
Matrix M(A);
|
||||
vector_scale_inplace(v, M, inf_mask);
|
||||
return M;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// column scaling
|
||||
Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
|
||||
Matrix M(A);
|
||||
const size_t n = A.cols();
|
||||
if (inf_mask) {
|
||||
for (size_t j=0; j<n; ++j) {
|
||||
const double& vj = v(j);
|
||||
if (!isnan(vj) && !isinf(vj))
|
||||
M.col(j) *= vj;
|
||||
}
|
||||
} else {
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M.col(j) *= v(j);
|
||||
}
|
||||
return M;
|
||||
Matrix M(A);
|
||||
const size_t n = A.cols();
|
||||
if (inf_mask) {
|
||||
for (size_t j=0; j<n; ++j) {
|
||||
const double& vj = v(j);
|
||||
if (!isnan(vj) && !isinf(vj))
|
||||
M.col(j) *= vj;
|
||||
}
|
||||
} else {
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M.col(j) *= v(j);
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 skewSymmetric(double wx, double wy, double wz)
|
||||
{
|
||||
return (Matrix3() <<
|
||||
0.0, -wz, +wy,
|
||||
+wz, 0.0, -wx,
|
||||
-wy, +wx, 0.0).finished();
|
||||
return (Matrix3() <<
|
||||
0.0, -wz, +wy,
|
||||
+wz, 0.0, -wx,
|
||||
-wy, +wx, 0.0).finished();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -592,11 +592,11 @@ Matrix3 skewSymmetric(double wx, double wy, double wz)
|
|||
/* ************************************************************************* */
|
||||
// FIXME: assumes row major, rather than column major
|
||||
//double** createNRC(Matrix& A) {
|
||||
// const size_t m=A.rows();
|
||||
// double** a = new double* [m];
|
||||
// for(size_t i = 0; i < m; i++)
|
||||
// a[i] = &A(i,0)-1;
|
||||
// return a;
|
||||
// const size_t m=A.rows();
|
||||
// double** a = new double* [m];
|
||||
// for(size_t i = 0; i < m; i++)
|
||||
// a[i] = &A(i,0)-1;
|
||||
// return a;
|
||||
//}
|
||||
|
||||
/* ******************************************
|
||||
|
|
@ -611,15 +611,15 @@ Matrix3 skewSymmetric(double wx, double wy, double wz)
|
|||
* ******************************************/
|
||||
Matrix LLt(const Matrix& A)
|
||||
{
|
||||
Matrix L = zeros(A.rows(), A.rows());
|
||||
Eigen::LLT<Matrix> llt;
|
||||
llt.compute(A);
|
||||
return llt.matrixL();
|
||||
Matrix L = zeros(A.rows(), A.rows());
|
||||
Eigen::LLT<Matrix> 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<m; i++) S(i) = - pow(S(i),-0.5);
|
||||
return vector_scale(S, V); // V*S;
|
||||
// invert and sqrt diagonal of S
|
||||
// We also arbitrarily choose sign to make result have positive signs
|
||||
for(size_t i = 0; i<m; i++) S(i) = - pow(S(i),-0.5);
|
||||
return vector_scale(S, V); // V*S;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -664,62 +664,62 @@ Matrix inverse_square_root(const Matrix& A) {
|
|||
// inv(B) * inv(B)' == A
|
||||
// inv(B' * B) == A
|
||||
Matrix inverse_square_root(const Matrix& A) {
|
||||
Matrix R = RtR(A);
|
||||
Matrix inv = eye(A.rows());
|
||||
R.triangularView<Eigen::Upper>().solveInPlace<Eigen::OnTheRight>(inv);
|
||||
return inv.transpose();
|
||||
Matrix R = RtR(A);
|
||||
Matrix inv = eye(A.rows());
|
||||
R.triangularView<Eigen::Upper>().solveInPlace<Eigen::OnTheRight>(inv);
|
||||
return inv.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) {
|
||||
Eigen::JacobiSVD<Matrix> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
|
||||
U = svd.matrixU();
|
||||
S = svd.singularValues();
|
||||
V = svd.matrixV();
|
||||
Eigen::JacobiSVD<Matrix> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
|
||||
U = svd.matrixU();
|
||||
S = svd.singularValues();
|
||||
V = svd.matrixV();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::tuple<int, double, Vector> 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<Matrix> 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<p ? 0 : s(m-1);
|
||||
return boost::tuple<int, double, Vector>(rank, error, Vector(column(V, p-1)));
|
||||
// Return rank, error, and corresponding column of V
|
||||
double error = m<p ? 0 : s(m-1);
|
||||
return boost::tuple<int, double, Vector>(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();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -100,19 +100,19 @@ Matrix diag(const Vector& v);
|
|||
template <class MATRIX>
|
||||
bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& 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<m1; i++)
|
||||
for(size_t j=0; j<n1; j++) {
|
||||
if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
|
||||
return false;
|
||||
else if(fabs(A(i,j) - B(i,j)) > tol)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
for(size_t i=0; i<m1; i++)
|
||||
for(size_t j=0; j<n1; j++) {
|
||||
if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
|
||||
return false;
|
||||
else if(fabs(A(i,j) - B(i,j)) > 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<class MATRIX>
|
||||
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<class MATRIX>
|
||||
Eigen::Block<const MATRIX> 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<class MATRIX>
|
||||
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<class MATRIX>
|
||||
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<class MATRIX>
|
||||
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<k; ++j)
|
||||
A.col(j).segment(j+1, m-(j+1)).setZero();
|
||||
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<k; ++j)
|
||||
A.col(j).segment(j+1, m-(j+1)).setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -313,18 +313,18 @@ std::pair<Matrix,Matrix> qr(const Matrix& A);
|
|||
*/
|
||||
template <class MATRIX>
|
||||
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<Matrix>::type HCoeffsType;
|
||||
typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
|
||||
HCoeffsType hCoeffs(size);
|
||||
RowVectorType temp(cols);
|
||||
typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
|
||||
typedef Eigen::internal::plain_row_type<Matrix>::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<int N>
|
||||
Matrix Cayley(const Eigen::Matrix<double, N, N>& A) {
|
||||
typedef Eigen::Matrix<double, N, N> FMat;
|
||||
return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse());
|
||||
typedef Eigen::Matrix<double, N, N> FMat;
|
||||
return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse());
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -488,31 +488,31 @@ Matrix Cayley(const Eigen::Matrix<double, N, N>& A) {
|
|||
#include <boost/serialization/split_free.hpp>
|
||||
|
||||
namespace boost {
|
||||
namespace serialization {
|
||||
namespace serialization {
|
||||
|
||||
// split version - sends sizes ahead
|
||||
template<class Archive>
|
||||
void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) {
|
||||
const int rows = m.rows(), cols = m.cols(), elements = rows * cols;
|
||||
std::vector<double> 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<class Archive>
|
||||
void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) {
|
||||
const int rows = m.rows(), cols = m.cols(), elements = rows * cols;
|
||||
std::vector<double> 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<class Archive>
|
||||
void load(Archive & ar, gtsam::Matrix & m, unsigned int version) {
|
||||
size_t rows, cols;
|
||||
std::vector<double> 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<class Archive>
|
||||
void load(Archive & ar, gtsam::Matrix & m, unsigned int version) {
|
||||
size_t rows, cols;
|
||||
std::vector<double> 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)
|
||||
|
|
|
|||
|
|
@ -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 T>
|
||||
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 T>
|
||||
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<class T>
|
||||
inline void print(const T& object, const std::string& s = "") {
|
||||
object.print(s);
|
||||
}
|
||||
/** Call print on the object */
|
||||
template<class T>
|
||||
inline void print(const T& object, const std::string& s = "") {
|
||||
object.print(s);
|
||||
}
|
||||
|
||||
/** Call equal on the object */
|
||||
template<class T>
|
||||
inline bool equal(const T& obj1, const T& obj2, double tol) {
|
||||
return obj1.equals(obj2, tol);
|
||||
}
|
||||
/** Call equal on the object */
|
||||
template<class T>
|
||||
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<class T>
|
||||
inline bool equal(const T& obj1, const T& obj2) {
|
||||
return obj1.equals(obj2);
|
||||
}
|
||||
/** Call equal on the object without tolerance (use default tolerance) */
|
||||
template<class T>
|
||||
inline bool equal(const T& obj1, const T& obj2) {
|
||||
return obj1.equals(obj2);
|
||||
}
|
||||
|
||||
/**
|
||||
* This template works for any type with equals
|
||||
*/
|
||||
template<class V>
|
||||
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<class V>
|
||||
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<class V>
|
||||
struct equals : public std::binary_function<const V&, const V&, bool> {
|
||||
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<class V>
|
||||
struct equals : public std::binary_function<const V&, const V&, bool> {
|
||||
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<class V>
|
||||
struct equals_star : public std::binary_function<const boost::shared_ptr<V>&, const boost::shared_ptr<V>&, bool> {
|
||||
double tol_;
|
||||
equals_star(double tol = 1e-9) : tol_(tol) {}
|
||||
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
|
||||
if (!actual || !expected) return false;
|
||||
return (actual->equals(*expected, tol_));
|
||||
}
|
||||
};
|
||||
/**
|
||||
* Binary predicate on shared pointers
|
||||
*/
|
||||
template<class V>
|
||||
struct equals_star : public std::binary_function<const boost::shared_ptr<V>&, const boost::shared_ptr<V>&, bool> {
|
||||
double tol_;
|
||||
equals_star(double tol = 1e-9) : tol_(tol) {}
|
||||
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
|
||||
if (!actual || !expected) return false;
|
||||
return (actual->equals(*expected, tol_));
|
||||
}
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -47,36 +47,36 @@ inline bool assert_equal(const Index& expected, const Index& actual, double tol
|
|||
*/
|
||||
template<class V>
|
||||
bool assert_equal(const boost::optional<V>& expected,
|
||||
const boost::optional<V>& 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<V>& 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<class V>
|
||||
bool assert_equal(const V& expected, const boost::optional<V>& 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<class V>
|
||||
bool assert_equal(const V& expected, const boost::optional<const V&>& 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<V>& expected, const std::vector<V>& 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<V>& expected, const std::vector<V>& actual,
|
|||
*/
|
||||
template<class V1, class V2>
|
||||
bool assert_container_equal(const std::map<V1,V2>& expected, const std::map<V1,V2>& actual, double tol = 1e-9) {
|
||||
typedef typename std::map<V1,V2> Map;
|
||||
typedef typename std::map<V1,V2> 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<V1,V2>& expected, const std::map<V1,V
|
|||
*/
|
||||
template<class V2>
|
||||
bool assert_container_equal(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual, double tol = 1e-9) {
|
||||
typedef typename std::map<size_t,V2> Map;
|
||||
typedef typename std::map<size_t,V2> 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<size_t,V2>& expected, const std::map<
|
|||
*/
|
||||
template<class V1, class V2>
|
||||
bool assert_container_equal(const std::vector<std::pair<V1,V2> >& expected,
|
||||
const std::vector<std::pair<V1,V2> >& actual, double tol = 1e-9) {
|
||||
typedef typename std::vector<std::pair<V1,V2> > VectorPair;
|
||||
const std::vector<std::pair<V1,V2> >& actual, double tol = 1e-9) {
|
||||
typedef typename std::vector<std::pair<V1,V2> > 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<class V2>
|
||||
bool assert_container_equality(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual) {
|
||||
typedef typename std::map<size_t,V2> Map;
|
||||
typedef typename std::map<size_t,V2> 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<class V>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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<Value> clone() const = 0;
|
||||
/** Clone this value (normal clone on the heap, delete with 'delete' operator) */
|
||||
virtual boost::shared_ptr<Value> 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<Value>(*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<class ARCHIVE>
|
||||
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<Value>(*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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {}
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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<double>& 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<n; i++)
|
||||
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
|
||||
odprintf_("];\n", stream);
|
||||
size_t n = v.size();
|
||||
odprintf_("%s [", stream, s.c_str());
|
||||
for(size_t i=0; i<n; i++)
|
||||
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
|
||||
odprintf_("];\n", stream);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void save(const Vector& v, const string &s, const string& filename) {
|
||||
fstream stream(filename.c_str(), fstream::out | fstream::app);
|
||||
print(v, s + "=", stream);
|
||||
stream.close();
|
||||
fstream stream(filename.c_str(), fstream::out | fstream::app);
|
||||
print(v, s + "=", stream);
|
||||
stream.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool operator==(const Vector& vec1,const Vector& vec2) {
|
||||
if (vec1.size() != vec2.size()) return false;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; i++)
|
||||
if(vec1[i] != vec2[i])
|
||||
return false;
|
||||
return true;
|
||||
if (vec1.size() != vec2.size()) return false;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; i++)
|
||||
if(vec1[i] != vec2[i])
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool greaterThanOrEqual(const Vector& vec1, const Vector& vec2) {
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; i++)
|
||||
if(!(vec1[i] >= vec2[i]))
|
||||
return false;
|
||||
return true;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; i++)
|
||||
if(!(vec1[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<m; ++i) {
|
||||
if(isnan(vec1[i]) ^ isnan(vec2[i]))
|
||||
return false;
|
||||
if(fabs(vec1[i] - vec2[i]) > tol)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
if (vec1.size()!=vec2.size()) return false;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; ++i) {
|
||||
if(isnan(vec1[i]) ^ isnan(vec2[i]))
|
||||
return false;
|
||||
if(fabs(vec1[i] - vec2[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<m; ++i) {
|
||||
if(isnan(vec1[i]) ^ isnan(vec2[i]))
|
||||
return false;
|
||||
if(fabs(vec1[i] - vec2[i]) > tol)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
if (vec1.size()!=vec2.size()) return false;
|
||||
size_t m = vec1.size();
|
||||
for(size_t i=0; i<m; ++i) {
|
||||
if(isnan(vec1[i]) ^ isnan(vec2[i]))
|
||||
return false;
|
||||
if(fabs(vec1[i] - vec2[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; i<m; i++) {
|
||||
if((fabs(vec1[i])>tol&&fabs(vec2[i])<tol) || (fabs(vec1[i])<tol&&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; i<m; i++) {
|
||||
if((fabs(vec1[i])>tol&&fabs(vec2[i])<tol) || (fabs(vec1[i])<tol&&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<double, Vector > 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<double>::infinity();
|
||||
size_t m = weights.size();
|
||||
static const double inf = std::numeric_limits<double>::infinity();
|
||||
|
||||
// Check once for zero entries of a. TODO: really needed ?
|
||||
vector<bool> 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<bool> 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<Vector, double>
|
||||
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<Vector>& 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<Vector> 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<Vector> 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<class V1, class V2>
|
||||
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<class V1, class V2>
|
||||
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<class V1, class V2>
|
||||
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<class Archive>
|
||||
void save(Archive & ar, const gtsam::Vector & v, unsigned int version)
|
||||
{
|
||||
const size_t n = v.size();
|
||||
std::vector<double> 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<double> raw_data(n);
|
||||
copy(v.data(), v.data()+n, raw_data.begin());
|
||||
ar << make_nvp("data", raw_data);
|
||||
}
|
||||
template<class Archive>
|
||||
void load(Archive & ar, gtsam::Vector & v, unsigned int version)
|
||||
{
|
||||
std::vector<double> raw_data;
|
||||
ar >> make_nvp("data", raw_data);
|
||||
v = gtsam::Vector_(raw_data);
|
||||
std::vector<double> raw_data;
|
||||
ar >> make_nvp("data", raw_data);
|
||||
v = gtsam::Vector_(raw_data);
|
||||
}
|
||||
|
||||
} // namespace serialization
|
||||
|
|
|
|||
|
|
@ -307,11 +307,11 @@ private:
|
|||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
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<class ARCHIVE>
|
||||
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_);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Eigen::Upper>()
|
||||
// .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<Eigen::Upper>()
|
||||
// .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<order; ++j)
|
||||
ATA(k,j) = 0.0;
|
||||
if(debug) cout << "choleskyCareful: Skipping " << k << endl;
|
||||
return 0;
|
||||
if(debug) cout << "choleskyCareful: Skipping " << k << endl;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -102,18 +102,18 @@ pair<size_t,bool> 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<Matrix, Eigen::Upper> llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt();
|
||||
Eigen::LLT<Matrix, Eigen::Upper> llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt();
|
||||
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() = 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<Eigen::Upper>()) << 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,23 +29,23 @@
|
|||
namespace gtsam {
|
||||
namespace testing {
|
||||
|
||||
/** binary functions */
|
||||
template<class T>
|
||||
T between(const T& t1, const T& t2) { return t1.between(t2); }
|
||||
/** binary functions */
|
||||
template<class T>
|
||||
T between(const T& t1, const T& t2) { return t1.between(t2); }
|
||||
|
||||
template<class T>
|
||||
T compose(const T& t1, const T& t2) { return t1.compose(t2); }
|
||||
template<class T>
|
||||
T compose(const T& t1, const T& t2) { return t1.compose(t2); }
|
||||
|
||||
/** unary functions */
|
||||
template<class T>
|
||||
T inverse(const T& t) { return t.inverse(); }
|
||||
/** unary functions */
|
||||
template<class T>
|
||||
T inverse(const T& t) { return t.inverse(); }
|
||||
|
||||
/** rotation functions */
|
||||
template<class T, class P>
|
||||
P rotate(const T& r, const P& pt) { return r.rotate(pt); }
|
||||
/** rotation functions */
|
||||
template<class T, class P>
|
||||
P rotate(const T& r, const P& pt) { return r.rotate(pt); }
|
||||
|
||||
template<class T, class P>
|
||||
P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
|
||||
template<class T, class P>
|
||||
P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
|
||||
|
||||
} // \namespace testing
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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<Matrix&> 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<Matrix&> 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<class X>
|
||||
Vector numericalGradient(boost::function<double(const X&)> 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<n;j++) {
|
||||
d(j) += delta; double hxplus = h(x.retract(d));
|
||||
d(j) -= 2*delta; double hxmin = h(x.retract(d));
|
||||
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
|
||||
}
|
||||
return g;
|
||||
}
|
||||
/**
|
||||
* Numerically compute gradient of scalar function
|
||||
* Class X is the input argument
|
||||
* The class X needs to have dim, expmap, logmap
|
||||
*/
|
||||
template<class X>
|
||||
Vector numericalGradient(boost::function<double(const X&)> 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<n;j++) {
|
||||
d(j) += delta; double hxplus = h(x.retract(d));
|
||||
d(j) -= 2*delta; double hxmin = h(x.retract(d));
|
||||
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
template<class X>
|
||||
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalGradient<X>(boost::bind(h, _1), x, delta);
|
||||
}
|
||||
template<class X>
|
||||
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalGradient<X>(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<class Y, class X>
|
||||
Matrix numericalDerivative11(boost::function<Y(const X&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
/**
|
||||
* 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<class Y, class X>
|
||||
Matrix numericalDerivative11(boost::function<Y(const X&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
|
||||
/** use a raw C++ function pointer */
|
||||
template<class Y, class X>
|
||||
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
|
||||
}
|
||||
/** use a raw C++ function pointer */
|
||||
template<class Y, class X>
|
||||
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
|
||||
}
|
||||
|
||||
/** remapping for double valued functions */
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
/** remapping for double valued functions */
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
|
||||
/** remapping for vector valued functions */
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(boost::function<Vector(const X&)> h, const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
/** remapping for vector valued functions */
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(boost::function<Vector(const X&)> h, const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
|
||||
}
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<LieVector, X>(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<class Y, class X1, class X2>
|
||||
Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
/**
|
||||
* 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<class Y, class X1, class X2>
|
||||
Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
|
||||
/** use a raw C++ function pointer */
|
||||
template<class Y, class X1, class X2>
|
||||
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
}
|
||||
/** use a raw C++ function pointer */
|
||||
template<class Y, class X1, class X2>
|
||||
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative21(boost::function<Vector(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative21(boost::function<Vector(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
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<class Y, class X1, class X2>
|
||||
Matrix numericalDerivative22
|
||||
(boost::function<Y(const X1&, const X2&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
/**
|
||||
* 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<class Y, class X1, class X2>
|
||||
Matrix numericalDerivative22
|
||||
(boost::function<Y(const X1&, const X2&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
|
||||
/** use a raw C++ function pointer */
|
||||
template<class Y, class X1, class X2>
|
||||
inline Matrix numericalDerivative22
|
||||
(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
}
|
||||
/** use a raw C++ function pointer */
|
||||
template<class Y, class X1, class X2>
|
||||
inline Matrix numericalDerivative22
|
||||
(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative22(boost::function<Vector(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2>
|
||||
Matrix numericalDerivative22(boost::function<Vector(const X1&, const X2&)> h,
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<LieVector,X1,X2>(
|
||||
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<class Y, class X1, class X2, class X3>
|
||||
Matrix numericalDerivative31
|
||||
(boost::function<Y(const X1&, const X2&, const X3&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
template<class Y, class X1, class X2, class X3>
|
||||
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<Y,X1,X2, X3>(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<class Y, class X1, class X2, class X3>
|
||||
Matrix numericalDerivative31
|
||||
(boost::function<Y(const X1&, const X2&, const X3&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
template<class Y, class X1, class X2, class X3>
|
||||
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<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative31(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative31<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative31(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative31<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative31(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative31<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative31(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative31<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
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<class Y, class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32
|
||||
(boost::function<Y(const X1&, const X2&, const X3&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
template<class Y, class X1, class X2, class X3>
|
||||
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<Y,X1,X2, X3>(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<class Y, class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32
|
||||
(boost::function<Y(const X1&, const X2&, const X3&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
template<class Y, class X1, class X2, class X3>
|
||||
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<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative32<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative32<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative32<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative32<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
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<class Y, class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33
|
||||
(boost::function<Y(const X1&, const X2&, const X3&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
template<class Y, class X1, class X2, class X3>
|
||||
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<Y,X1,X2, X3>(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<class Y, class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33
|
||||
(boost::function<Y(const X1&, const X2&, const X3&)> 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<n;j++) {
|
||||
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
|
||||
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
template<class Y, class X1, class X2, class X3>
|
||||
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<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for double return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
template<class X1, class X2, class X3>
|
||||
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<LieVector,X1,X2,X3>(
|
||||
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<class X>
|
||||
inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<X>(boost::function<Vector(const X&)>(boost::bind(
|
||||
static_cast<Vector (*)(boost::function<double(const X&)>,const X&, double)>(&numericalGradient<X>),
|
||||
f, _1, delta)), x, delta);
|
||||
}
|
||||
template<class X>
|
||||
inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x, double delta=1e-5) {
|
||||
return numericalDerivative11<X>(boost::function<Vector(const X&)>(boost::bind(
|
||||
static_cast<Vector (*)(boost::function<double(const X&)>,const X&, double)>(&numericalGradient<X>),
|
||||
f, _1, delta)), x, delta);
|
||||
}
|
||||
|
||||
template<class X>
|
||||
inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalHessian(boost::function<double(const X&)>(f), x, delta);
|
||||
}
|
||||
template<class X>
|
||||
inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) {
|
||||
return numericalHessian(boost::function<double(const X&)>(f), x, delta);
|
||||
}
|
||||
|
||||
|
||||
/** Helper class that computes the derivative of f w.r.t. x1, centered about
|
||||
|
|
|
|||
|
|
@ -23,15 +23,15 @@ using namespace gtsam;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(testBlockMatrices, jacobian_factor1) {
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> 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<AbMatrix> BlockAb;
|
||||
typedef Matrix AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> 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<InfoMatrix> 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))));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<DSFVector::V> v = boost::make_shared<DSFVector::V>(5);
|
||||
std::vector<size_t> keys; keys += 1, 3;
|
||||
DSFVector dsf(v, keys);
|
||||
dsf.makeUnionInPlace(1,3);
|
||||
CHECK(dsf.findSet(1) == dsf.findSet(3));
|
||||
boost::shared_ptr<DSFVector::V> v = boost::make_shared<DSFVector::V>(5);
|
||||
std::vector<size_t> 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<size_t, set<size_t> > sets = dsf.sets();
|
||||
LONGS_EQUAL(1, sets.size());
|
||||
DSFVector dsf(2);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, set<size_t> > sets = dsf.sets();
|
||||
LONGS_EQUAL(1, sets.size());
|
||||
|
||||
set<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == sets[dsf.findSet(0)]);
|
||||
set<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == sets[dsf.findSet(0)]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(DSFVector, arrays) {
|
||||
DSFVector dsf(2);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
LONGS_EQUAL(1, arrays.size());
|
||||
DSFVector dsf(2);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
LONGS_EQUAL(1, arrays.size());
|
||||
|
||||
vector<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == arrays[dsf.findSet(0)]);
|
||||
vector<size_t> 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<size_t, set<size_t> > sets = dsf.sets();
|
||||
LONGS_EQUAL(1, sets.size());
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
dsf.makeUnionInPlace(1,2);
|
||||
map<size_t, set<size_t> > sets = dsf.sets();
|
||||
LONGS_EQUAL(1, sets.size());
|
||||
|
||||
set<size_t> expected; expected += 0, 1, 2;
|
||||
CHECK(expected == sets[dsf.findSet(0)]);
|
||||
set<size_t> 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<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
LONGS_EQUAL(1, arrays.size());
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
dsf.makeUnionInPlace(1,2);
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
LONGS_EQUAL(1, arrays.size());
|
||||
|
||||
vector<size_t> expected; expected += 0, 1, 2;
|
||||
CHECK(expected == arrays[dsf.findSet(0)]);
|
||||
vector<size_t> expected; expected += 0, 1, 2;
|
||||
CHECK(expected == arrays[dsf.findSet(0)]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(DSFVector, sets3) {
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, set<size_t> > sets = dsf.sets();
|
||||
LONGS_EQUAL(2, sets.size());
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, set<size_t> > sets = dsf.sets();
|
||||
LONGS_EQUAL(2, sets.size());
|
||||
|
||||
set<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == sets[dsf.findSet(0)]);
|
||||
set<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == sets[dsf.findSet(0)]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(DSFVector, arrays3) {
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
LONGS_EQUAL(2, arrays.size());
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
LONGS_EQUAL(2, arrays.size());
|
||||
|
||||
vector<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == arrays[dsf.findSet(0)]);
|
||||
vector<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == arrays[dsf.findSet(0)]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(DSFVector, set) {
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
set<size_t> set = dsf.set(0);
|
||||
LONGS_EQUAL(2, set.size());
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
set<size_t> set = dsf.set(0);
|
||||
LONGS_EQUAL(2, set.size());
|
||||
|
||||
std::set<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == set);
|
||||
std::set<size_t> expected; expected += 0, 1;
|
||||
CHECK(expected == set);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(DSFVector, set2) {
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
dsf.makeUnionInPlace(1,2);
|
||||
set<size_t> set = dsf.set(0);
|
||||
LONGS_EQUAL(3, set.size());
|
||||
DSFVector dsf(3);
|
||||
dsf.makeUnionInPlace(0,1);
|
||||
dsf.makeUnionInPlace(1,2);
|
||||
set<size_t> set = dsf.set(0);
|
||||
LONGS_EQUAL(3, set.size());
|
||||
|
||||
std::set<size_t> expected; expected += 0, 1, 2;
|
||||
CHECK(expected == set);
|
||||
std::set<size_t> 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);}
|
||||
|
|
|
|||
|
|
@ -21,12 +21,12 @@ using namespace gtsam;
|
|||
/* ************************************************************************* */
|
||||
TEST( testFastContainers, KeySet ) {
|
||||
|
||||
FastVector<size_t> init_vector;
|
||||
init_vector += 2, 3, 4, 5;
|
||||
FastVector<size_t> init_vector;
|
||||
init_vector += 2, 3, 4, 5;
|
||||
|
||||
FastSet<size_t> actSet(init_vector);
|
||||
FastSet<size_t> expSet; expSet += 2, 3, 4, 5;
|
||||
EXPECT(actSet == expSet);
|
||||
FastSet<size_t> actSet(init_vector);
|
||||
FastSet<size_t> expSet; expSet += 2, 3, 4, 5;
|
||||
EXPECT(actSet == expSet);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -23,12 +23,12 @@ using namespace gtsam;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( testTestableAssertions, optional ) {
|
||||
typedef boost::optional<LieScalar> 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<LieScalar> 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<double>::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<double>::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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<const Matrix *> matrices;
|
||||
for (size_t i=0; i<p;++i) {
|
||||
Matrix * M = new Matrix;
|
||||
(*M) = eye(m,n);
|
||||
matrices.push_back(M);
|
||||
}
|
||||
// fill the matrices with identities
|
||||
vector<const Matrix *> matrices;
|
||||
for (size_t i=0; i<p;++i) {
|
||||
Matrix * M = new Matrix;
|
||||
(*M) = eye(m,n);
|
||||
matrices.push_back(M);
|
||||
}
|
||||
|
||||
// start timing
|
||||
Matrix result;
|
||||
double elapsed;
|
||||
{
|
||||
boost::timer t;
|
||||
// start timing
|
||||
Matrix result;
|
||||
double elapsed;
|
||||
{
|
||||
boost::timer t;
|
||||
|
||||
if (passDims)
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
result = collect(matrices, m, n);
|
||||
else
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
result = collect(matrices);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
// delete the matrices
|
||||
for (size_t i=0; i<p;++i) {
|
||||
delete matrices[i];
|
||||
}
|
||||
if (passDims)
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
result = collect(matrices, m, n);
|
||||
else
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
result = collect(matrices);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
// delete the matrices
|
||||
for (size_t i=0; i<p;++i) {
|
||||
delete matrices[i];
|
||||
}
|
||||
|
||||
return elapsed;
|
||||
//return elapsed/reps;
|
||||
return elapsed;
|
||||
//return elapsed/reps;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -81,27 +81,27 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) {
|
|||
* - rev 2100 : 0.52 sec (x1000)
|
||||
*/
|
||||
double timeVScaleColumn(size_t m, size_t n, size_t reps) {
|
||||
// make a matrix to scale
|
||||
Matrix M(m, n);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M(i,j) = 2*i+j;
|
||||
// make a matrix to scale
|
||||
Matrix M(m, n);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M(i,j) = 2*i+j;
|
||||
|
||||
// make a vector to use for scaling
|
||||
Vector V(m);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
V(i) = i*2;
|
||||
// make a vector to use for scaling
|
||||
Vector V(m);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
V(i) = i*2;
|
||||
|
||||
double elapsed;
|
||||
Matrix result;
|
||||
{
|
||||
boost::timer t;
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
Matrix result = vector_scale(M,V);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
double elapsed;
|
||||
Matrix result;
|
||||
{
|
||||
boost::timer t;
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
Matrix result = vector_scale(M,V);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
|
||||
return elapsed;
|
||||
return elapsed;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -112,27 +112,27 @@ double timeVScaleColumn(size_t m, size_t n, size_t reps) {
|
|||
* - rev 2100 : 1.69 sec (x1000)
|
||||
*/
|
||||
double timeVScaleRow(size_t m, size_t n, size_t reps) {
|
||||
// make a matrix to scale
|
||||
Matrix M(m, n);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M(i,j) = 2*i+j;
|
||||
// make a matrix to scale
|
||||
Matrix M(m, n);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M(i,j) = 2*i+j;
|
||||
|
||||
// make a vector to use for scaling
|
||||
Vector V(n);
|
||||
for (size_t i=0; i<n; ++i)
|
||||
V(i) = i*2;
|
||||
// make a vector to use for scaling
|
||||
Vector V(n);
|
||||
for (size_t i=0; i<n; ++i)
|
||||
V(i) = i*2;
|
||||
|
||||
double elapsed;
|
||||
Matrix result;
|
||||
{
|
||||
boost::timer t;
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
result = vector_scale(V,M);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
double elapsed;
|
||||
Matrix result;
|
||||
{
|
||||
boost::timer t;
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
result = vector_scale(V,M);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
|
||||
return elapsed;
|
||||
return elapsed;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -145,25 +145,25 @@ double timeVScaleRow(size_t m, size_t n, size_t reps) {
|
|||
* - rev 2100 : 45.11 sec
|
||||
*/
|
||||
double timeColumn(size_t reps) {
|
||||
// create a matrix
|
||||
size_t m = 100; size_t n = 100;
|
||||
Matrix M(m, n);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M(i,j) = 2*i+j;
|
||||
// create a matrix
|
||||
size_t m = 100; size_t n = 100;
|
||||
Matrix M(m, n);
|
||||
for (size_t i=0; i<m; ++i)
|
||||
for (size_t j=0; j<n; ++j)
|
||||
M(i,j) = 2*i+j;
|
||||
|
||||
// extract a column
|
||||
double elapsed;
|
||||
Vector result;
|
||||
{
|
||||
boost::timer t;
|
||||
for (size_t i=0; i<reps; ++i)
|
||||
for (size_t j = 0; j<n; ++j)
|
||||
//result = ublas::matrix_column<Matrix>(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<reps; ++i)
|
||||
for (size_t j = 0; j<n; ++j)
|
||||
//result = ublas::matrix_column<Matrix>(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<reps; ++i) {
|
||||
Matrix A = Abase;
|
||||
householder_(A,3);
|
||||
}
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
return elapsed;
|
||||
// perform timing
|
||||
double elapsed;
|
||||
{
|
||||
boost::timer t;
|
||||
for (size_t i=0; i<reps; ++i) {
|
||||
Matrix A = Abase;
|
||||
householder_(A,3);
|
||||
}
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
return elapsed;
|
||||
}
|
||||
/**
|
||||
* Results: (Alex's machine)
|
||||
|
|
@ -215,63 +215,63 @@ double timeHouseholder(size_t reps) {
|
|||
* Direct pointer method - 4.62
|
||||
*/
|
||||
double timeMatrixInsert(size_t reps) {
|
||||
// create a matrix
|
||||
Matrix bigBase = zeros(100, 100);
|
||||
Matrix small = eye(5,5);
|
||||
// create a matrix
|
||||
Matrix bigBase = zeros(100, 100);
|
||||
Matrix small = eye(5,5);
|
||||
|
||||
// perform timing
|
||||
double elapsed;
|
||||
{
|
||||
boost::timer t;
|
||||
Matrix big = bigBase;
|
||||
for (size_t rep=0; rep<reps; ++rep)
|
||||
for (size_t i=0; i<100; i += 5)
|
||||
for (size_t j=0; j<100; j += 5)
|
||||
insertSub(big, small, i,j);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
return elapsed;
|
||||
// perform timing
|
||||
double elapsed;
|
||||
{
|
||||
boost::timer t;
|
||||
Matrix big = bigBase;
|
||||
for (size_t rep=0; rep<reps; ++rep)
|
||||
for (size_t i=0; i<100; i += 5)
|
||||
for (size_t j=0; j<100; j += 5)
|
||||
insertSub(big, small, i,j);
|
||||
elapsed = t.elapsed();
|
||||
}
|
||||
return elapsed;
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv) {
|
||||
|
||||
// Time collect()
|
||||
cout << "Starting Matrix::collect() Timing" << endl;
|
||||
//size_t p = 100000; size_t m = 10; size_t n = 12; size_t reps = 50;
|
||||
size_t p = 10; size_t m = 10; size_t n = 12; size_t reps = 10000000;
|
||||
double collect_time1 = timeCollect(p, m, n, false, reps);
|
||||
double collect_time2 = timeCollect(p, m, n, true, reps);
|
||||
cout << "Average Elapsed time for collect (no pass) [" << p << " (" << m << ", " << n << ") matrices] : " << collect_time1 << endl;
|
||||
cout << "Average Elapsed time for collect (pass) [" << p << " (" << m << ", " << n << ") matrices] : " << collect_time2 << endl;
|
||||
// Time collect()
|
||||
cout << "Starting Matrix::collect() Timing" << endl;
|
||||
//size_t p = 100000; size_t m = 10; size_t n = 12; size_t reps = 50;
|
||||
size_t p = 10; size_t m = 10; size_t n = 12; size_t reps = 10000000;
|
||||
double collect_time1 = timeCollect(p, m, n, false, reps);
|
||||
double collect_time2 = timeCollect(p, m, n, true, reps);
|
||||
cout << "Average Elapsed time for collect (no pass) [" << p << " (" << m << ", " << n << ") matrices] : " << collect_time1 << endl;
|
||||
cout << "Average Elapsed time for collect (pass) [" << p << " (" << m << ", " << n << ") matrices] : " << collect_time2 << endl;
|
||||
|
||||
// Time vector_scale_column
|
||||
cout << "Starting Matrix::vector_scale(column) Timing" << endl;
|
||||
size_t m1 = 400; size_t n1 = 480; size_t reps1 = 1000;
|
||||
double vsColumn_time = timeVScaleColumn(m1, n1, reps1);
|
||||
cout << "Elapsed time for vector_scale(column) [(" << m1 << ", " << n1 << ") matrix] : " << vsColumn_time << endl;
|
||||
// Time vector_scale_column
|
||||
cout << "Starting Matrix::vector_scale(column) Timing" << endl;
|
||||
size_t m1 = 400; size_t n1 = 480; size_t reps1 = 1000;
|
||||
double vsColumn_time = timeVScaleColumn(m1, n1, reps1);
|
||||
cout << "Elapsed time for vector_scale(column) [(" << m1 << ", " << n1 << ") matrix] : " << vsColumn_time << endl;
|
||||
|
||||
// Time vector_scale_row
|
||||
cout << "Starting Matrix::vector_scale(row) Timing" << endl;
|
||||
double vsRow_time = timeVScaleRow(m1, n1, reps1);
|
||||
cout << "Elapsed time for vector_scale(row) [(" << m1 << ", " << n1 << ") matrix] : " << vsRow_time << endl;
|
||||
// Time vector_scale_row
|
||||
cout << "Starting Matrix::vector_scale(row) Timing" << endl;
|
||||
double vsRow_time = timeVScaleRow(m1, n1, reps1);
|
||||
cout << "Elapsed time for vector_scale(row) [(" << m1 << ", " << n1 << ") matrix] : " << vsRow_time << endl;
|
||||
|
||||
// Time column() NOTE: using the ublas version
|
||||
cout << "Starting column() Timing" << endl;
|
||||
size_t reps2 = 2000000;
|
||||
double column_time = timeColumn(reps2);
|
||||
cout << "Time: " << column_time << " sec" << endl;
|
||||
// Time column() NOTE: using the ublas version
|
||||
cout << "Starting column() Timing" << endl;
|
||||
size_t reps2 = 2000000;
|
||||
double column_time = timeColumn(reps2);
|
||||
cout << "Time: " << column_time << " sec" << endl;
|
||||
|
||||
// Time householder_ function
|
||||
cout << "Starting householder_() Timing" << endl;
|
||||
size_t reps_house = 5000000;
|
||||
double house_time = timeHouseholder(reps_house);
|
||||
cout << "Elapsed time for householder_() : " << house_time << " sec" << endl;
|
||||
// Time householder_ function
|
||||
cout << "Starting householder_() Timing" << endl;
|
||||
size_t reps_house = 5000000;
|
||||
double house_time = timeHouseholder(reps_house);
|
||||
cout << "Elapsed time for householder_() : " << house_time << " sec" << endl;
|
||||
|
||||
// Time matrix insertion
|
||||
cout << "Starting insertSub() Timing" << endl;
|
||||
size_t reps_insert = 200000;
|
||||
double insert_time = timeMatrixInsert(reps_insert);
|
||||
cout << "Elapsed time for insertSub() : " << insert_time << " sec" << endl;
|
||||
// Time matrix insertion
|
||||
cout << "Starting insertSub() Timing" << endl;
|
||||
size_t reps_insert = 200000;
|
||||
double insert_time = timeMatrixInsert(reps_insert);
|
||||
cout << "Elapsed time for insertSub() : " << insert_time << " sec" << endl;
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@
|
|||
* @file timing.cpp
|
||||
* @brief Timing utilities
|
||||
* @author Richard Roberts, Michael Kaess
|
||||
* @date Oct 5, 2010
|
||||
* @date Oct 5, 2010
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
|
|
@ -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<TimingOutline>& 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<TimingOutline>& 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<children_.size(); ++i) {
|
||||
if(children_[i]) {
|
||||
std::string childOutline(outline);
|
||||
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<children_.size(); ++i) {
|
||||
if(children_[i]) {
|
||||
std::string childOutline(outline);
|
||||
#if 0
|
||||
if(childOutline.size() > 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>& TimingOutline::child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& 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; i<children_.size(); ++i)
|
||||
if(children_[i])
|
||||
children_[i]->finishedIteration();
|
||||
if(tIt_ > tMax_)
|
||||
tMax_ = tIt_;
|
||||
if(tMin_ == 0 || tIt_ < tMin_)
|
||||
tMin_ = tIt_;
|
||||
tIt_ = 0;
|
||||
for(size_t i=0; i<children_.size(); ++i)
|
||||
if(children_[i])
|
||||
children_[i]->finishedIteration();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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<TimingOutline> 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<TimingOutline> 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<std::string, Timing::Stats>::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<std::string, Timing::Stats>::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);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
|
||||
typename AS_CONST>
|
||||
struct const_selector {
|
||||
};
|
||||
/**
|
||||
* Helper class that uses templates to select between two types based on
|
||||
* whether TEST_TYPE is const or not.
|
||||
*/
|
||||
template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
|
||||
typename AS_CONST>
|
||||
struct const_selector {
|
||||
};
|
||||
|
||||
/** Specialization for the non-const version */
|
||||
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
|
||||
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
|
||||
typedef AS_NON_CONST type;
|
||||
};
|
||||
/** Specialization for the non-const version */
|
||||
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
|
||||
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
|
||||
typedef AS_NON_CONST type;
|
||||
};
|
||||
|
||||
/** Specialization for the const version */
|
||||
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
|
||||
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
|
||||
typedef AS_CONST type;
|
||||
};
|
||||
/** Specialization for the const version */
|
||||
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
|
||||
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
|
||||
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<typename T, T defaultValue>
|
||||
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<typename T, T defaultValue>
|
||||
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; }
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<typename L>
|
||||
class AlgebraicDecisionTree: public DecisionTree<L, double> {
|
||||
/**
|
||||
* Algebraic Decision Trees fix the range to double
|
||||
* Just has some nice constructors and some syntactic sugar
|
||||
* TODO: consider eliminating this class altogether?
|
||||
*/
|
||||
template<typename L>
|
||||
class AlgebraicDecisionTree: public DecisionTree<L, double> {
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
typedef DecisionTree<L, double> Super;
|
||||
typedef DecisionTree<L, double> 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<typename Super::LabelC>& labelCs, const std::vector<double>& ys) {
|
||||
this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(),
|
||||
ys.end());
|
||||
}
|
||||
/** Create from keys and vector table */
|
||||
AlgebraicDecisionTree //
|
||||
(const std::vector<typename Super::LabelC>& labelCs, const std::vector<double>& ys) {
|
||||
this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(),
|
||||
ys.end());
|
||||
}
|
||||
|
||||
/** Create from keys and string table */
|
||||
AlgebraicDecisionTree //
|
||||
(const std::vector<typename Super::LabelC>& labelCs, const std::string& table) {
|
||||
// Convert string to doubles
|
||||
std::vector<double> ys;
|
||||
std::istringstream iss(table);
|
||||
std::copy(std::istream_iterator<double>(iss),
|
||||
std::istream_iterator<double>(), std::back_inserter(ys));
|
||||
/** Create from keys and string table */
|
||||
AlgebraicDecisionTree //
|
||||
(const std::vector<typename Super::LabelC>& labelCs, const std::string& table) {
|
||||
// Convert string to doubles
|
||||
std::vector<double> ys;
|
||||
std::istringstream iss(table);
|
||||
std::copy(std::istream_iterator<double>(iss),
|
||||
std::istream_iterator<double>(), 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<typename Iterator>
|
||||
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<typename Iterator>
|
||||
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
|
||||
Super(NULL) {
|
||||
this->root_ = compose(begin, end, label);
|
||||
}
|
||||
|
||||
/** Convert */
|
||||
template<typename M>
|
||||
AlgebraicDecisionTree(const AlgebraicDecisionTree<M>& other,
|
||||
const std::map<M, L>& map) {
|
||||
this->root_ = this->template convert<M, double>(other.root_, map,
|
||||
Ring::id);
|
||||
}
|
||||
/** Convert */
|
||||
template<typename M>
|
||||
AlgebraicDecisionTree(const AlgebraicDecisionTree<M>& other,
|
||||
const std::map<M, L>& map) {
|
||||
this->root_ = this->template convert<M, double>(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
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 L>
|
||||
class Assignment: public std::map<L, size_t> {
|
||||
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 L>
|
||||
class Assignment: public std::map<L, size_t> {
|
||||
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<typename L>
|
||||
std::vector<Assignment<L> > cartesianProduct(
|
||||
const std::vector<std::pair<L, size_t> >& keys) {
|
||||
std::vector<Assignment<L> > allPossValues;
|
||||
Assignment<L> values;
|
||||
typedef std::pair<L, size_t> 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<typename L>
|
||||
std::vector<Assignment<L> > cartesianProduct(
|
||||
const std::vector<std::pair<L, size_t> >& keys) {
|
||||
std::vector<Assignment<L> > allPossValues;
|
||||
Assignment<L> values;
|
||||
typedef std::pair<L, size_t> 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
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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<typename L, typename Y>
|
||||
class DecisionTree {
|
||||
/**
|
||||
* Algebraic Decision Trees
|
||||
* L = label for variables
|
||||
* Y = function range (any algebra), e.g., bool, int, double
|
||||
*/
|
||||
template<typename L, typename Y>
|
||||
class DecisionTree {
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** Handy typedefs for unary and binary function types */
|
||||
typedef boost::function<Y(const Y&)> Unary;
|
||||
typedef boost::function<Y(const Y&, const Y&)> Binary;
|
||||
/** Handy typedefs for unary and binary function types */
|
||||
typedef boost::function<Y(const Y&)> Unary;
|
||||
typedef boost::function<Y(const Y&, const Y&)> Binary;
|
||||
|
||||
/** A label annotated with cardinality */
|
||||
typedef std::pair<L,size_t> LabelC;
|
||||
/** A label annotated with cardinality */
|
||||
typedef std::pair<L,size_t> 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<const Node> Ptr;
|
||||
/** ------------------------ Node base class --------------------------- */
|
||||
class Node {
|
||||
public:
|
||||
typedef boost::shared_ptr<const Node> 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<L>& 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<L>& 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<typename It, typename ValueIt>
|
||||
NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const;
|
||||
/** Internal recursive function to create from keys, cardinalities, and Y values */
|
||||
template<typename It, typename ValueIt>
|
||||
NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const;
|
||||
|
||||
/** Convert to a different type */
|
||||
template<typename M, typename X> NodePtr
|
||||
convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
|
||||
L>& map, boost::function<Y(const X&)> op);
|
||||
/** Convert to a different type */
|
||||
template<typename M, typename X> NodePtr
|
||||
convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
|
||||
L>& map, boost::function<Y(const X&)> 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<LabelC>& labelCs, const std::vector<Y>& ys);
|
||||
/** Create from keys and string table */
|
||||
DecisionTree(const std::vector<LabelC>& labelCs, const std::vector<Y>& ys);
|
||||
|
||||
/** Create from keys and string table */
|
||||
DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table);
|
||||
/** Create from keys and string table */
|
||||
DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table);
|
||||
|
||||
/** Create DecisionTree from others */
|
||||
template<typename Iterator>
|
||||
DecisionTree(Iterator begin, Iterator end, const L& label);
|
||||
/** Create DecisionTree from others */
|
||||
template<typename Iterator>
|
||||
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<typename M, typename X>
|
||||
DecisionTree(const DecisionTree<M, X>& other,
|
||||
const std::map<M, L>& map, boost::function<Y(const X&)> op);
|
||||
/** Convert from a different type */
|
||||
template<typename M, typename X>
|
||||
DecisionTree(const DecisionTree<M, X>& other,
|
||||
const std::map<M, L>& map, boost::function<Y(const X&)> 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<L>& x) const;
|
||||
/** evaluate */
|
||||
const Y& operator()(const Assignment<L>& 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<typename Iterator> NodePtr
|
||||
compose(Iterator begin, Iterator end, const L& label) const;
|
||||
// internal use only
|
||||
template<typename Iterator> NodePtr
|
||||
compose(Iterator begin, Iterator end, const L& label) const;
|
||||
|
||||
/// @}
|
||||
/// @}
|
||||
|
||||
}; // DecisionTree
|
||||
}; // DecisionTree
|
||||
|
||||
/** free versions of apply */
|
||||
/** free versions of apply */
|
||||
|
||||
template<typename Y, typename L>
|
||||
DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
|
||||
const typename DecisionTree<L, Y>::Unary& op) {
|
||||
return f.apply(op);
|
||||
}
|
||||
template<typename Y, typename L>
|
||||
DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
|
||||
const typename DecisionTree<L, Y>::Unary& op) {
|
||||
return f.apply(op);
|
||||
}
|
||||
|
||||
template<typename Y, typename L>
|
||||
DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
|
||||
const DecisionTree<L, Y>& g,
|
||||
const typename DecisionTree<L, Y>::Binary& op) {
|
||||
return f.apply(g, op);
|
||||
}
|
||||
template<typename Y, typename L>
|
||||
DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
|
||||
const DecisionTree<L, Y>& g,
|
||||
const typename DecisionTree<L, Y>::Binary& op) {
|
||||
return f.apply(g, op);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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<Index,size_t> 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<Index,size_t> 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<DecisionTreeFactor>(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<DecisionTreeFactor>(dkeys, result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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<DecisionTreeFactor> shared_ptr;
|
||||
// typedefs needed to play nice with gtsam
|
||||
typedef DecisionTreeFactor This;
|
||||
typedef DiscreteConditional ConditionalType;
|
||||
typedef boost::shared_ptr<DecisionTreeFactor> 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<class SOURCE>
|
||||
DecisionTreeFactor(const DiscreteKeys& keys, SOURCE table) :
|
||||
DiscreteFactor(keys.indices()), Potentials(keys, table) {
|
||||
}
|
||||
/** Constructor from Indices and (string or doubles) */
|
||||
template<class SOURCE>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -23,18 +23,18 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class BayesNet<DiscreteConditional> ;
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class BayesNet<DiscreteConditional> ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void add_front(DiscreteBayesNet& bayesNet, const Signature& s) {
|
||||
bayesNet.push_front(boost::make_shared<DiscreteConditional>(s));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void add_front(DiscreteBayesNet& bayesNet, const Signature& s) {
|
||||
bayesNet.push_front(boost::make_shared<DiscreteConditional>(s));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void add(DiscreteBayesNet& bayesNet, const Signature& s) {
|
||||
bayesNet.push_back(boost::make_shared<DiscreteConditional>(s));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void add(DiscreteBayesNet& bayesNet, const Signature& s) {
|
||||
bayesNet.push_back(boost::make_shared<DiscreteConditional>(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
|
||||
|
|
|
|||
|
|
@ -25,22 +25,22 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
typedef BayesNet<DiscreteConditional> DiscreteBayesNet;
|
||||
typedef BayesNet<DiscreteConditional> 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
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Values> allPosbValues = cartesianProduct(keys);
|
||||
DiscreteKeys keys;
|
||||
BOOST_FOREACH(Index idx, frontals()) {
|
||||
DiscreteKey dk(idx,cardinality(idx));
|
||||
keys & dk;
|
||||
}
|
||||
// Get all Possible Configurations
|
||||
vector<Values> 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<double> 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<double> 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<boost::mt19937&, uniform_real<> > 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<boost::mt19937&, uniform_real<> > 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);
|
||||
}
|
||||
|
||||
|
||||
/* ******************************************************************************** */
|
||||
|
|
|
|||
|
|
@ -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<DiscreteConditional> shared_ptr;
|
||||
typedef IndexConditional Base;
|
||||
public:
|
||||
// typedefs needed to play nice with gtsam
|
||||
typedef DiscreteFactor FactorType;
|
||||
typedef boost::shared_ptr<DiscreteConditional> shared_ptr;
|
||||
typedef IndexConditional Base;
|
||||
|
||||
/** A map from keys to values */
|
||||
typedef Assignment<Index> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
/** A map from keys to values */
|
||||
typedef Assignment<Index> Values;
|
||||
typedef boost::shared_ptr<Values> 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<DiscreteConditional>.
|
||||
* @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr<DiscreteConditional>.
|
||||
* */
|
||||
template<typename ITERATOR>
|
||||
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<DiscreteConditional>.
|
||||
* @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr<DiscreteConditional>.
|
||||
* */
|
||||
template<typename ITERATOR>
|
||||
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<Index>& parentsValues) const;
|
||||
/** Restrict to given parent values, returns AlgebraicDecisionDiagram */
|
||||
ADT choose(const Assignment<Index>& 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<typename ITERATOR>
|
||||
DiscreteConditional::shared_ptr DiscreteConditional::Combine(
|
||||
ITERATOR firstConditional, ITERATOR lastConditional) {
|
||||
// TODO: check for being a clique
|
||||
/* ************************************************************************* */
|
||||
template<typename ITERATOR>
|
||||
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<DiscreteConditional>(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<DiscreteConditional>(nrFrontals,product);
|
||||
}
|
||||
|
||||
}// gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -24,9 +24,9 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ******************************************************************************** */
|
||||
DiscreteFactor::DiscreteFactor() {
|
||||
}
|
||||
/* ******************************************************************************** */
|
||||
DiscreteFactor::DiscreteFactor() {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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<DiscreteFactor> shared_ptr;
|
||||
// typedefs needed to play nice with gtsam
|
||||
typedef DiscreteFactor This;
|
||||
typedef DiscreteConditional ConditionalType;
|
||||
typedef boost::shared_ptr<DiscreteFactor> shared_ptr;
|
||||
|
||||
/** A map from keys to values */
|
||||
typedef Assignment<Index> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
/** A map from keys to values */
|
||||
typedef Assignment<Index> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
|
||||
protected:
|
||||
protected:
|
||||
|
||||
/// Construct n-way factor
|
||||
DiscreteFactor(const std::vector<Index>& js) :
|
||||
IndexFactor(js) {
|
||||
}
|
||||
/// Construct n-way factor
|
||||
DiscreteFactor(const std::vector<Index>& 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<class KeyIterator>
|
||||
DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) :
|
||||
IndexFactor(beginKey, endKey) {
|
||||
}
|
||||
/// construct from container
|
||||
template<class KeyIterator>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -24,56 +24,56 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class FactorGraph<DiscreteFactor> ;
|
||||
template class EliminationTree<DiscreteFactor> ;
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class FactorGraph<DiscreteFactor> ;
|
||||
template class EliminationTree<DiscreteFactor> ;
|
||||
|
||||
/* ************************************************************************* */
|
||||
DiscreteFactorGraph::DiscreteFactorGraph() {
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
DiscreteFactorGraph::DiscreteFactorGraph() {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
DiscreteFactorGraph::DiscreteFactorGraph(
|
||||
const BayesNet<DiscreteConditional>& bayesNet) :
|
||||
FactorGraph<DiscreteFactor>(bayesNet) {
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
DiscreteFactorGraph::DiscreteFactorGraph(
|
||||
const BayesNet<DiscreteConditional>& bayesNet) :
|
||||
FactorGraph<DiscreteFactor>(bayesNet) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> DiscreteFactorGraph::keys() const {
|
||||
FastSet<Index> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this)
|
||||
if (factor) keys.insert(factor->begin(), factor->end());
|
||||
return keys;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> DiscreteFactorGraph::keys() const {
|
||||
FastSet<Index> 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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
|
||||
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) {
|
||||
/* ************************************************************************* */
|
||||
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
|
||||
EliminateDiscrete(const FactorGraph<DiscreteFactor>& 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);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -29,56 +29,56 @@ namespace gtsam {
|
|||
class DiscreteFactorGraph: public FactorGraph<DiscreteFactor> {
|
||||
public:
|
||||
|
||||
/** A map from keys to values */
|
||||
typedef std::vector<Index> Indices;
|
||||
typedef Assignment<Index> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
/** A map from keys to values */
|
||||
typedef std::vector<Index> Indices;
|
||||
typedef Assignment<Index> Values;
|
||||
typedef boost::shared_ptr<Values> sharedValues;
|
||||
|
||||
/** Construct empty factor graph */
|
||||
DiscreteFactorGraph();
|
||||
/** Construct empty factor graph */
|
||||
DiscreteFactorGraph();
|
||||
|
||||
/** Constructor from a factor graph of GaussianFactor or a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) {
|
||||
push_back(fg);
|
||||
}
|
||||
/** Constructor from a factor graph of GaussianFactor or a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) {
|
||||
push_back(fg);
|
||||
}
|
||||
|
||||
/** construct from a BayesNet */
|
||||
DiscreteFactorGraph(const BayesNet<DiscreteConditional>& bayesNet);
|
||||
/** construct from a BayesNet */
|
||||
DiscreteFactorGraph(const BayesNet<DiscreteConditional>& bayesNet);
|
||||
|
||||
template<class SOURCE>
|
||||
void add(const DiscreteKey& j, SOURCE table) {
|
||||
DiscreteKeys keys;
|
||||
keys.push_back(j);
|
||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
||||
}
|
||||
template<class SOURCE>
|
||||
void add(const DiscreteKey& j, SOURCE table) {
|
||||
DiscreteKeys keys;
|
||||
keys.push_back(j);
|
||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
||||
}
|
||||
|
||||
template<class SOURCE>
|
||||
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<DecisionTreeFactor>(keys, table));
|
||||
}
|
||||
template<class SOURCE>
|
||||
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<DecisionTreeFactor>(keys, table));
|
||||
}
|
||||
|
||||
/** add shared discreteFactor immediately from arguments */
|
||||
template<class SOURCE>
|
||||
void add(const DiscreteKeys& keys, SOURCE table) {
|
||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
||||
}
|
||||
/** add shared discreteFactor immediately from arguments */
|
||||
template<class SOURCE>
|
||||
void add(const DiscreteKeys& keys, SOURCE table) {
|
||||
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
|
||||
}
|
||||
|
||||
/** Return the set of variables involved in the factors (set union) */
|
||||
FastSet<Index> keys() const;
|
||||
/** Return the set of variables involved in the factors (set union) */
|
||||
FastSet<Index> 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<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr>
|
||||
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors,
|
||||
size_t nrFrontals = 1);
|
||||
size_t nrFrontals = 1);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -23,33 +23,33 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
using namespace std;
|
||||
|
||||
DiscreteKeys::DiscreteKeys(const vector<int>& 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<int>& 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<Index> DiscreteKeys::indices() const {
|
||||
vector < Index > js;
|
||||
BOOST_FOREACH(const DiscreteKey& key, *this)
|
||||
js.push_back(key.first);
|
||||
return js;
|
||||
}
|
||||
vector<Index> DiscreteKeys::indices() const {
|
||||
vector < Index > js;
|
||||
BOOST_FOREACH(const DiscreteKey& key, *this)
|
||||
js.push_back(key.first);
|
||||
return js;
|
||||
}
|
||||
|
||||
map<Index,size_t> DiscreteKeys::cardinalities() const {
|
||||
map<Index,size_t> cs;
|
||||
cs.insert(begin(),end());
|
||||
// BOOST_FOREACH(const DiscreteKey& key, *this)
|
||||
// cs.insert(key);
|
||||
return cs;
|
||||
}
|
||||
map<Index,size_t> DiscreteKeys::cardinalities() const {
|
||||
map<Index,size_t> 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue