global find/replace. Does not compile
parent
dd95757229
commit
d1d5336ed0
|
@ -48,16 +48,16 @@ int main(const int argc, const char *argv[]) {
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
bool is3D = false;
|
bool is3D = false;
|
||||||
if (kernelType.compare("none") == 0) {
|
if (kernelType.compare("none") == 0) {
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
std::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
}
|
}
|
||||||
if (kernelType.compare("huber") == 0) {
|
if (kernelType.compare("huber") == 0) {
|
||||||
std::cout << "Using robust kernel: huber " << std::endl;
|
std::cout << "Using robust kernel: huber " << std::endl;
|
||||||
boost::tie(graph, initial) =
|
std::tie(graph, initial) =
|
||||||
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
|
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
|
||||||
}
|
}
|
||||||
if (kernelType.compare("tukey") == 0) {
|
if (kernelType.compare("tukey") == 0) {
|
||||||
std::cout << "Using robust kernel: tukey " << std::endl;
|
std::cout << "Using robust kernel: tukey " << std::endl;
|
||||||
boost::tie(graph, initial) =
|
std::tie(graph, initial) =
|
||||||
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
|
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -90,7 +90,7 @@ int main(const int argc, const char *argv[]) {
|
||||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||||
Values::shared_ptr initial2;
|
Values::shared_ptr initial2;
|
||||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||||
writeG2o(*graphNoKernel, result, outputFile);
|
writeG2o(*graphNoKernel, result, outputFile);
|
||||||
std::cout << "done! " << std::endl;
|
std::cout << "done! " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,7 +36,7 @@ int main (int argc, char** argv) {
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
|
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
|
||||||
string graph_file = findExampleDataFile("w100.graph");
|
string graph_file = findExampleDataFile("w100.graph");
|
||||||
boost::tie(graph, initial) = load2D(graph_file, model);
|
std::tie(graph, initial) = load2D(graph_file, model);
|
||||||
initial->print("Initial estimate:\n");
|
initial->print("Initial estimate:\n");
|
||||||
|
|
||||||
// Add a Gaussian prior on first poses
|
// Add a Gaussian prior on first poses
|
||||||
|
|
|
@ -37,7 +37,7 @@ int main(const int argc, const char *argv[]) {
|
||||||
|
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile);
|
std::tie(graph, initial) = readG2o(g2oFile);
|
||||||
|
|
||||||
// Add prior on the pose having index (key) = 0
|
// Add prior on the pose having index (key) = 0
|
||||||
auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
|
auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
|
||||||
|
@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
|
||||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||||
Values::shared_ptr initial2;
|
Values::shared_ptr initial2;
|
||||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||||
writeG2o(*graphNoKernel, estimateLago, outputFile);
|
writeG2o(*graphNoKernel, estimateLago, outputFile);
|
||||||
std::cout << "done! " << std::endl;
|
std::cout << "done! " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
|
@ -37,7 +37,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
std::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
// Add prior on the first key
|
// Add prior on the first key
|
||||||
auto priorModel = noiseModel::Diagonal::Variances(
|
auto priorModel = noiseModel::Diagonal::Variances(
|
||||||
|
@ -67,7 +67,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||||
Values::shared_ptr initial2;
|
Values::shared_ptr initial2;
|
||||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||||
writeG2o(*graphNoKernel, result, outputFile);
|
writeG2o(*graphNoKernel, result, outputFile);
|
||||||
std::cout << "done! " << std::endl;
|
std::cout << "done! " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,7 +36,7 @@ int main(const int argc, const char *argv[]) {
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
std::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
bool add = false;
|
bool add = false;
|
||||||
Key firstKey = 8646911284551352320;
|
Key firstKey = 8646911284551352320;
|
||||||
|
|
|
@ -36,7 +36,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
std::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
// Add prior on the first key
|
// Add prior on the first key
|
||||||
auto priorModel = noiseModel::Diagonal::Variances(
|
auto priorModel = noiseModel::Diagonal::Variances(
|
||||||
|
@ -66,7 +66,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||||
Values::shared_ptr initial2;
|
Values::shared_ptr initial2;
|
||||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||||
writeG2o(*graphNoKernel, result, outputFile);
|
writeG2o(*graphNoKernel, result, outputFile);
|
||||||
std::cout << "done! " << std::endl;
|
std::cout << "done! " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,7 +36,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
std::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
// Add prior on the first key
|
// Add prior on the first key
|
||||||
auto priorModel = noiseModel::Diagonal::Variances(
|
auto priorModel = noiseModel::Diagonal::Variances(
|
||||||
|
@ -60,7 +60,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||||
Values::shared_ptr initial2;
|
Values::shared_ptr initial2;
|
||||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||||
writeG2o(*graphNoKernel, initialization, outputFile);
|
writeG2o(*graphNoKernel, initialization, outputFile);
|
||||||
std::cout << "done! " << std::endl;
|
std::cout << "done! " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,7 +36,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
std::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
// Add prior on the first key
|
// Add prior on the first key
|
||||||
auto priorModel = noiseModel::Diagonal::Variances(
|
auto priorModel = noiseModel::Diagonal::Variances(
|
||||||
|
@ -66,7 +66,7 @@ int main(const int argc, const char* argv[]) {
|
||||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||||
Values::shared_ptr initial2;
|
Values::shared_ptr initial2;
|
||||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||||
writeG2o(*graphNoKernel, initialization, outputFile);
|
writeG2o(*graphNoKernel, initialization, outputFile);
|
||||||
std::cout << "done! " << std::endl;
|
std::cout << "done! " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
|
@ -92,7 +92,7 @@ std::list<TimedOdometry> readOdometry() {
|
||||||
|
|
||||||
// load the ranges from TD
|
// load the ranges from TD
|
||||||
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||||
using RangeTriple = boost::tuple<double, size_t, double>;
|
using RangeTriple = std::tuple<double, size_t, double>;
|
||||||
std::vector<RangeTriple> readTriples() {
|
std::vector<RangeTriple> readTriples() {
|
||||||
std::vector<RangeTriple> triples;
|
std::vector<RangeTriple> triples;
|
||||||
std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt");
|
std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt");
|
||||||
|
@ -166,7 +166,7 @@ int main(int argc, char** argv) {
|
||||||
//--------------------------------- odometry loop --------------------------
|
//--------------------------------- odometry loop --------------------------
|
||||||
double t;
|
double t;
|
||||||
Pose2 odometry;
|
Pose2 odometry;
|
||||||
boost::tie(t, odometry) = timedOdometry;
|
std::tie(t, odometry) = timedOdometry;
|
||||||
|
|
||||||
// add odometry factor
|
// add odometry factor
|
||||||
newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(i - 1, i, odometry,
|
newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(i - 1, i, odometry,
|
||||||
|
|
|
@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
|
||||||
auto result = shonan.run(initial, pMin);
|
auto result = shonan.run(initial, pMin);
|
||||||
|
|
||||||
// Parse file again to set up translation problem, adding a prior
|
// Parse file again to set up translation problem, adding a prior
|
||||||
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
|
std::tie(inputGraph, posesInFile) = load2D(inputFile);
|
||||||
auto priorModel = noiseModel::Unit::Create(3);
|
auto priorModel = noiseModel::Unit::Create(3);
|
||||||
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
|
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
|
||||||
|
|
||||||
|
@ -119,7 +119,7 @@ int main(int argc, char* argv[]) {
|
||||||
auto result = shonan.run(initial, pMin);
|
auto result = shonan.run(initial, pMin);
|
||||||
|
|
||||||
// Parse file again to set up translation problem, adding a prior
|
// Parse file again to set up translation problem, adding a prior
|
||||||
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
|
std::tie(inputGraph, posesInFile) = load3D(inputFile);
|
||||||
auto priorModel = noiseModel::Unit::Create(6);
|
auto priorModel = noiseModel::Unit::Create(6);
|
||||||
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
|
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
|
||||||
|
|
||||||
|
|
|
@ -251,7 +251,7 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
|
||||||
|
|
||||||
// calculate the Householder vector v
|
// calculate the Householder vector v
|
||||||
double beta; Vector vjm;
|
double beta; Vector vjm;
|
||||||
boost::tie(beta,vjm) = house(xjm);
|
std::tie(beta,vjm) = house(xjm);
|
||||||
|
|
||||||
// pad with zeros to get m-dimensional vector v
|
// pad with zeros to get m-dimensional vector v
|
||||||
for(size_t k = 0 ; k < m; k++)
|
for(size_t k = 0 ; k < m; k++)
|
||||||
|
@ -269,13 +269,13 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
list<boost::tuple<Vector, double, double> >
|
list<std::tuple<Vector, double, double> >
|
||||||
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
|
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
|
||||||
size_t m = A.rows(), n = A.cols(); // get size(A)
|
size_t m = A.rows(), n = A.cols(); // get size(A)
|
||||||
size_t maxRank = min(m,n);
|
size_t maxRank = min(m,n);
|
||||||
|
|
||||||
// create list
|
// create list
|
||||||
list<boost::tuple<Vector, double, double> > results;
|
list<std::tuple<Vector, double, double> > results;
|
||||||
|
|
||||||
Vector pseudo(m); // allocate storage for pseudo-inverse
|
Vector pseudo(m); // allocate storage for pseudo-inverse
|
||||||
Vector weights = sigmas.array().square().inverse(); // calculate weights once
|
Vector weights = sigmas.array().square().inverse(); // calculate weights once
|
||||||
|
@ -304,7 +304,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
|
||||||
|
|
||||||
// construct solution (r, d, sigma)
|
// construct solution (r, d, sigma)
|
||||||
// TODO: avoid sqrt, store precision or at least variance
|
// TODO: avoid sqrt, store precision or at least variance
|
||||||
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
|
results.push_back(std::make_tuple(r, d, 1./sqrt(precision)));
|
||||||
|
|
||||||
// exit after rank exhausted
|
// exit after rank exhausted
|
||||||
if (results.size()>=maxRank) break;
|
if (results.size()>=maxRank) break;
|
||||||
|
@ -565,7 +565,7 @@ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
|
std::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
|
||||||
|
|
||||||
// Check size of A
|
// Check size of A
|
||||||
size_t n = A.rows(), p = A.cols(), m = min(n,p);
|
size_t n = A.rows(), p = A.cols(), m = min(n,p);
|
||||||
|
@ -582,7 +582,7 @@ boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
|
||||||
|
|
||||||
// Return rank, error, and corresponding column of V
|
// Return rank, error, and corresponding column of V
|
||||||
double error = m<p ? 0 : s(m-1);
|
double error = m<p ? 0 : s(m-1);
|
||||||
return boost::tuple<int, double, Vector>((int)rank, error, Vector(column(V, p-1)));
|
return std::tuple<int, double, Vector>((int)rank, error, Vector(column(V, p-1)));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -307,7 +307,7 @@ GTSAM_EXPORT void inplace_QR(Matrix& A);
|
||||||
* @param sigmas is a vector of the measurement standard deviation
|
* @param sigmas is a vector of the measurement standard deviation
|
||||||
* @return list of r vectors, d and sigma
|
* @return list of r vectors, d and sigma
|
||||||
*/
|
*/
|
||||||
GTSAM_EXPORT std::list<boost::tuple<Vector, double, double> >
|
GTSAM_EXPORT std::list<std::tuple<Vector, double, double> >
|
||||||
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
|
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -434,7 +434,7 @@ GTSAM_EXPORT void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
|
||||||
* Returns rank of A, minimum error (singular value),
|
* Returns rank of A, minimum error (singular value),
|
||||||
* and corresponding eigenvector (column of V, with A=U*S*V')
|
* and corresponding eigenvector (column of V, with A=U*S*V')
|
||||||
*/
|
*/
|
||||||
GTSAM_EXPORT boost::tuple<int, double, Vector>
|
GTSAM_EXPORT std::tuple<int, double, Vector>
|
||||||
DLT(const Matrix& A, double rank_tol = 1e-9);
|
DLT(const Matrix& A, double rank_tol = 1e-9);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -859,7 +859,7 @@ TEST(Matrix, qr )
|
||||||
7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished();
|
7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished();
|
||||||
|
|
||||||
Matrix Q, R;
|
Matrix Q, R;
|
||||||
boost::tie(Q, R) = qr(A);
|
std::tie(Q, R) = qr(A);
|
||||||
EXPECT(assert_equal(expectedQ, Q, 1e-4));
|
EXPECT(assert_equal(expectedQ, Q, 1e-4));
|
||||||
EXPECT(assert_equal(expectedR, R, 1e-4));
|
EXPECT(assert_equal(expectedR, R, 1e-4));
|
||||||
EXPECT(assert_equal(A, Q*R, 1e-14));
|
EXPECT(assert_equal(A, Q*R, 1e-14));
|
||||||
|
@ -911,7 +911,7 @@ TEST(Matrix, weighted_elimination )
|
||||||
// perform elimination
|
// perform elimination
|
||||||
Matrix A1 = A;
|
Matrix A1 = A;
|
||||||
Vector b1 = b;
|
Vector b1 = b;
|
||||||
std::list<boost::tuple<Vector, double, double> > solution =
|
std::list<std::tuple<Vector, double, double> > solution =
|
||||||
weighted_eliminate(A1, b1, sigmas);
|
weighted_eliminate(A1, b1, sigmas);
|
||||||
|
|
||||||
// unpack and verify
|
// unpack and verify
|
||||||
|
@ -919,7 +919,7 @@ TEST(Matrix, weighted_elimination )
|
||||||
for (const auto& tuple : solution) {
|
for (const auto& tuple : solution) {
|
||||||
Vector r;
|
Vector r;
|
||||||
double di, sigma;
|
double di, sigma;
|
||||||
boost::tie(r, di, sigma) = tuple;
|
std::tie(r, di, sigma) = tuple;
|
||||||
EXPECT(assert_equal(r, expectedR.row(i))); // verify r
|
EXPECT(assert_equal(r, expectedR.row(i))); // verify r
|
||||||
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
|
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
|
||||||
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
|
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
|
||||||
|
@ -1146,7 +1146,7 @@ TEST(Matrix, DLT )
|
||||||
int rank;
|
int rank;
|
||||||
double error;
|
double error;
|
||||||
Vector actual;
|
Vector actual;
|
||||||
boost::tie(rank,error,actual) = DLT(A);
|
std::tie(rank,error,actual) = DLT(A);
|
||||||
Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0).finished();
|
Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0).finished();
|
||||||
EXPECT_LONGS_EQUAL(8,rank);
|
EXPECT_LONGS_EQUAL(8,rank);
|
||||||
EXPECT_DOUBLES_EQUAL(0,error,1e-8);
|
EXPECT_DOUBLES_EQUAL(0,error,1e-8);
|
||||||
|
|
|
@ -156,7 +156,7 @@ TEST(Vector, weightedPseudoinverse )
|
||||||
|
|
||||||
// perform solve
|
// perform solve
|
||||||
Vector actual; double precision;
|
Vector actual; double precision;
|
||||||
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
|
std::tie(actual, precision) = weightedPseudoinverse(x, weights);
|
||||||
|
|
||||||
// construct expected
|
// construct expected
|
||||||
Vector expected(2);
|
Vector expected(2);
|
||||||
|
@ -181,7 +181,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
|
||||||
Vector weights = sigmas.array().square().inverse();
|
Vector weights = sigmas.array().square().inverse();
|
||||||
// perform solve
|
// perform solve
|
||||||
Vector actual; double precision;
|
Vector actual; double precision;
|
||||||
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
|
std::tie(actual, precision) = weightedPseudoinverse(x, weights);
|
||||||
|
|
||||||
// construct expected
|
// construct expected
|
||||||
Vector expected(2);
|
Vector expected(2);
|
||||||
|
@ -199,7 +199,7 @@ TEST(Vector, weightedPseudoinverse_nan )
|
||||||
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
|
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
|
||||||
Vector weights = sigmas.array().square().inverse();
|
Vector weights = sigmas.array().square().inverse();
|
||||||
Vector pseudo; double precision;
|
Vector pseudo; double precision;
|
||||||
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
|
std::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
|
||||||
|
|
||||||
Vector expected = (Vector(4) << 1., 0., 0.,0.).finished();
|
Vector expected = (Vector(4) << 1., 0., 0.,0.).finished();
|
||||||
EXPECT(assert_equal(expected, pseudo));
|
EXPECT(assert_equal(expected, pseudo));
|
||||||
|
|
|
@ -111,7 +111,7 @@ TEST(DiscreteFactorGraph, test) {
|
||||||
frontalKeys += Key(0);
|
frontalKeys += Key(0);
|
||||||
DiscreteConditional::shared_ptr conditional;
|
DiscreteConditional::shared_ptr conditional;
|
||||||
DecisionTreeFactor::shared_ptr newFactor;
|
DecisionTreeFactor::shared_ptr newFactor;
|
||||||
boost::tie(conditional, newFactor) = EliminateDiscrete(graph, frontalKeys);
|
std::tie(conditional, newFactor) = EliminateDiscrete(graph, frontalKeys);
|
||||||
|
|
||||||
// Check Conditional
|
// Check Conditional
|
||||||
CHECK(conditional);
|
CHECK(conditional);
|
||||||
|
@ -130,7 +130,7 @@ TEST(DiscreteFactorGraph, test) {
|
||||||
DiscreteEliminationTree etree(graph, ordering);
|
DiscreteEliminationTree etree(graph, ordering);
|
||||||
DiscreteBayesNet::shared_ptr actual;
|
DiscreteBayesNet::shared_ptr actual;
|
||||||
DiscreteFactorGraph::shared_ptr remainingGraph;
|
DiscreteFactorGraph::shared_ptr remainingGraph;
|
||||||
boost::tie(actual, remainingGraph) = etree.eliminate(&EliminateDiscrete);
|
std::tie(actual, remainingGraph) = etree.eliminate(&EliminateDiscrete);
|
||||||
|
|
||||||
// Check Bayes net
|
// Check Bayes net
|
||||||
DiscreteBayesNet expectedBayesNet;
|
DiscreteBayesNet expectedBayesNet;
|
||||||
|
|
|
@ -170,13 +170,13 @@ Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
Matrix39 qHm;
|
Matrix39 qHm;
|
||||||
boost::tie(I, q) = RQ(m, qHm);
|
std::tie(I, q) = RQ(m, qHm);
|
||||||
|
|
||||||
// TODO : Explore whether this expression can be optimized as both
|
// TODO : Explore whether this expression can be optimized as both
|
||||||
// qHm and mH are super-sparse
|
// qHm and mH are super-sparse
|
||||||
*H = qHm * mH;
|
*H = qHm * mH;
|
||||||
} else
|
} else
|
||||||
boost::tie(I, q) = RQ(matrix());
|
std::tie(I, q) = RQ(matrix());
|
||||||
return q;
|
return q;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -510,7 +510,7 @@ TEST( Rot3, RQ)
|
||||||
// Try RQ on a pure rotation
|
// Try RQ on a pure rotation
|
||||||
Matrix actualK;
|
Matrix actualK;
|
||||||
Vector actual;
|
Vector actual;
|
||||||
boost::tie(actualK, actual) = RQ(R.matrix());
|
std::tie(actualK, actual) = RQ(R.matrix());
|
||||||
Vector expected = Vector3(0.14715, 0.385821, 0.231671);
|
Vector expected = Vector3(0.14715, 0.385821, 0.231671);
|
||||||
CHECK(assert_equal(I_3x3,actualK));
|
CHECK(assert_equal(I_3x3,actualK));
|
||||||
CHECK(assert_equal(expected,actual,1e-6));
|
CHECK(assert_equal(expected,actual,1e-6));
|
||||||
|
@ -531,7 +531,7 @@ TEST( Rot3, RQ)
|
||||||
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
|
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
|
||||||
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished();
|
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished();
|
||||||
Matrix A = K * R.matrix();
|
Matrix A = K * R.matrix();
|
||||||
boost::tie(actualK, actual) = RQ(A);
|
std::tie(actualK, actual) = RQ(A);
|
||||||
CHECK(assert_equal(K,actualK));
|
CHECK(assert_equal(K,actualK));
|
||||||
CHECK(assert_equal(expected,actual,1e-6));
|
CHECK(assert_equal(expected,actual,1e-6));
|
||||||
}
|
}
|
||||||
|
|
|
@ -51,7 +51,7 @@ Vector4 triangulateHomogeneousDLT(
|
||||||
int rank;
|
int rank;
|
||||||
double error;
|
double error;
|
||||||
Vector v;
|
Vector v;
|
||||||
boost::tie(rank, error, v) = DLT(A, rank_tol);
|
std::tie(rank, error, v) = DLT(A, rank_tol);
|
||||||
|
|
||||||
if (rank < 3) throw(TriangulationUnderconstrainedException());
|
if (rank < 3) throw(TriangulationUnderconstrainedException());
|
||||||
|
|
||||||
|
@ -82,7 +82,7 @@ Vector4 triangulateHomogeneousDLT(
|
||||||
int rank;
|
int rank;
|
||||||
double error;
|
double error;
|
||||||
Vector v;
|
Vector v;
|
||||||
boost::tie(rank, error, v) = DLT(A, rank_tol);
|
std::tie(rank, error, v) = DLT(A, rank_tol);
|
||||||
|
|
||||||
if (rank < 3) throw(TriangulationUnderconstrainedException());
|
if (rank < 3) throw(TriangulationUnderconstrainedException());
|
||||||
|
|
||||||
|
|
|
@ -195,7 +195,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
|
||||||
// Create a factor graph and initial values
|
// Create a factor graph and initial values
|
||||||
Values values;
|
Values values;
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
|
std::tie(graph, values) = triangulationGraph<CALIBRATION> //
|
||||||
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
|
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
|
||||||
|
|
||||||
return optimize(graph, values, Symbol('p', 0));
|
return optimize(graph, values, Symbol('p', 0));
|
||||||
|
@ -217,7 +217,7 @@ Point3 triangulateNonlinear(
|
||||||
// Create a factor graph and initial values
|
// Create a factor graph and initial values
|
||||||
Values values;
|
Values values;
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
boost::tie(graph, values) = triangulationGraph<CAMERA> //
|
std::tie(graph, values) = triangulationGraph<CAMERA> //
|
||||||
(cameras, measurements, Symbol('p', 0), initialEstimate, model);
|
(cameras, measurements, Symbol('p', 0), initialEstimate, model);
|
||||||
|
|
||||||
return optimize(graph, values, Symbol('p', 0));
|
return optimize(graph, values, Symbol('p', 0));
|
||||||
|
|
|
@ -102,7 +102,7 @@ struct HybridConstructorTraversalData {
|
||||||
keyAsOrdering.push_back(node->key);
|
keyAsOrdering.push_back(node->key);
|
||||||
SymbolicConditional::shared_ptr conditional;
|
SymbolicConditional::shared_ptr conditional;
|
||||||
SymbolicFactor::shared_ptr separatorFactor;
|
SymbolicFactor::shared_ptr separatorFactor;
|
||||||
boost::tie(conditional, separatorFactor) =
|
std::tie(conditional, separatorFactor) =
|
||||||
internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
|
internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
|
||||||
|
|
||||||
// Store symbolic elimination results in the parent
|
// Store symbolic elimination results in the parent
|
||||||
|
|
|
@ -361,7 +361,7 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
// Factor into C1\B | B.
|
// Factor into C1\B | B.
|
||||||
sharedFactorGraph temp_remaining;
|
sharedFactorGraph temp_remaining;
|
||||||
boost::tie(p_C1_B, temp_remaining) =
|
std::tie(p_C1_B, temp_remaining) =
|
||||||
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
|
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
|
||||||
}
|
}
|
||||||
std::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
|
std::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
|
||||||
|
@ -373,7 +373,7 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
// Factor into C2\B | B.
|
// Factor into C2\B | B.
|
||||||
sharedFactorGraph temp_remaining;
|
sharedFactorGraph temp_remaining;
|
||||||
boost::tie(p_C2_B, temp_remaining) =
|
std::tie(p_C2_B, temp_remaining) =
|
||||||
FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
|
FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
|
||||||
}
|
}
|
||||||
gttoc(Full_root_factoring);
|
gttoc(Full_root_factoring);
|
||||||
|
|
|
@ -75,7 +75,7 @@ namespace gtsam {
|
||||||
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
|
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
|
||||||
std::shared_ptr<BayesNetType> bayesNet;
|
std::shared_ptr<BayesNetType> bayesNet;
|
||||||
std::shared_ptr<FactorGraphType> factorGraph;
|
std::shared_ptr<FactorGraphType> factorGraph;
|
||||||
boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
|
std::tie(bayesNet,factorGraph) = etree.eliminate(function);
|
||||||
// If any factors are remaining, the ordering was incomplete
|
// If any factors are remaining, the ordering was incomplete
|
||||||
if(!factorGraph->empty())
|
if(!factorGraph->empty())
|
||||||
throw InconsistentEliminationRequested();
|
throw InconsistentEliminationRequested();
|
||||||
|
@ -139,7 +139,7 @@ namespace gtsam {
|
||||||
JunctionTreeType junctionTree(etree);
|
JunctionTreeType junctionTree(etree);
|
||||||
std::shared_ptr<BayesTreeType> bayesTree;
|
std::shared_ptr<BayesTreeType> bayesTree;
|
||||||
std::shared_ptr<FactorGraphType> factorGraph;
|
std::shared_ptr<FactorGraphType> factorGraph;
|
||||||
boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
|
std::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
|
||||||
// If any factors are remaining, the ordering was incomplete
|
// If any factors are remaining, the ordering was incomplete
|
||||||
if(!factorGraph->empty())
|
if(!factorGraph->empty())
|
||||||
throw InconsistentEliminationRequested();
|
throw InconsistentEliminationRequested();
|
||||||
|
@ -277,7 +277,7 @@ namespace gtsam {
|
||||||
// in the order requested.
|
// in the order requested.
|
||||||
std::shared_ptr<BayesTreeType> bayesTree;
|
std::shared_ptr<BayesTreeType> bayesTree;
|
||||||
std::shared_ptr<FactorGraphType> factorGraph;
|
std::shared_ptr<FactorGraphType> factorGraph;
|
||||||
boost::tie(bayesTree,factorGraph) =
|
std::tie(bayesTree,factorGraph) =
|
||||||
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
|
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
|
||||||
|
|
||||||
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
|
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
|
||||||
|
@ -344,7 +344,7 @@ namespace gtsam {
|
||||||
// in the order requested.
|
// in the order requested.
|
||||||
std::shared_ptr<BayesTreeType> bayesTree;
|
std::shared_ptr<BayesTreeType> bayesTree;
|
||||||
std::shared_ptr<FactorGraphType> factorGraph;
|
std::shared_ptr<FactorGraphType> factorGraph;
|
||||||
boost::tie(bayesTree,factorGraph) =
|
std::tie(bayesTree,factorGraph) =
|
||||||
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
|
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
|
||||||
|
|
||||||
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
|
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
|
||||||
|
|
|
@ -85,7 +85,7 @@ struct ConstructorTraversalData {
|
||||||
keyAsOrdering.push_back(ETreeNode->key);
|
keyAsOrdering.push_back(ETreeNode->key);
|
||||||
SymbolicConditional::shared_ptr myConditional;
|
SymbolicConditional::shared_ptr myConditional;
|
||||||
SymbolicFactor::shared_ptr mySeparatorFactor;
|
SymbolicFactor::shared_ptr mySeparatorFactor;
|
||||||
boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(
|
std::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(
|
||||||
symbolicFactors, keyAsOrdering);
|
symbolicFactors, keyAsOrdering);
|
||||||
|
|
||||||
// Store symbolic elimination results in the parent
|
// Store symbolic elimination results in the parent
|
||||||
|
|
|
@ -111,7 +111,7 @@ VariableSlots::VariableSlots(const FG& factorGraph)
|
||||||
// the array entry for each factor that will indicate the factor
|
// the array entry for each factor that will indicate the factor
|
||||||
// does not involve the variable.
|
// does not involve the variable.
|
||||||
iterator thisVarSlots; bool inserted;
|
iterator thisVarSlots; bool inserted;
|
||||||
boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
|
std::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
|
||||||
if(inserted)
|
if(inserted)
|
||||||
thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
|
thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
|
||||||
thisVarSlots->second[jointFactorPos] = factorVarSlot;
|
thisVarSlots->second[jointFactorPos] = factorVarSlot;
|
||||||
|
|
|
@ -54,7 +54,7 @@ std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map) {
|
||||||
SGraph<KEY> g;
|
SGraph<KEY> g;
|
||||||
SVertex root;
|
SVertex root;
|
||||||
std::map<KEY, SVertex> key2vertex;
|
std::map<KEY, SVertex> key2vertex;
|
||||||
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
|
std::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
|
||||||
|
|
||||||
// breadth first visit on the graph
|
// breadth first visit on the graph
|
||||||
std::list<KEY> keys;
|
std::list<KEY> keys;
|
||||||
|
@ -114,7 +114,7 @@ SDGraph<KEY> toBoostGraph(const G& graph) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template<class G, class V, class KEY>
|
template<class G, class V, class KEY>
|
||||||
boost::tuple<G, V, std::map<KEY,V> >
|
std::tuple<G, V, std::map<KEY,V> >
|
||||||
predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
|
predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
|
||||||
|
|
||||||
G g;
|
G g;
|
||||||
|
@ -146,7 +146,7 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
|
||||||
if (!foundRoot)
|
if (!foundRoot)
|
||||||
throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!");
|
throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!");
|
||||||
else
|
else
|
||||||
return boost::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
|
return std::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -185,7 +185,7 @@ std::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>&
|
||||||
PoseGraph g;
|
PoseGraph g;
|
||||||
PoseVertex root;
|
PoseVertex root;
|
||||||
std::map<KEY, PoseVertex> key2vertex;
|
std::map<KEY, PoseVertex> key2vertex;
|
||||||
boost::tie(g, root, key2vertex) =
|
std::tie(g, root, key2vertex) =
|
||||||
predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
|
predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
|
||||||
|
|
||||||
// attach the relative poses to the edges
|
// attach the relative poses to the edges
|
||||||
|
@ -207,8 +207,8 @@ std::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>&
|
||||||
PoseVertex v2 = key2vertex.find(key2)->second;
|
PoseVertex v2 = key2vertex.find(key2)->second;
|
||||||
|
|
||||||
POSE l1Xl2 = factor->measured();
|
POSE l1Xl2 = factor->measured();
|
||||||
boost::tie(edge12, found1) = boost::edge(v1, v2, g);
|
std::tie(edge12, found1) = boost::edge(v1, v2, g);
|
||||||
boost::tie(edge21, found2) = boost::edge(v2, v1, g);
|
std::tie(edge21, found2) = boost::edge(v2, v1, g);
|
||||||
if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree");
|
if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree");
|
||||||
if (!found1 && !found2) continue;
|
if (!found1 && !found2) continue;
|
||||||
if (found1)
|
if (found1)
|
||||||
|
|
|
@ -83,7 +83,7 @@ namespace gtsam {
|
||||||
* V = Vertex type
|
* V = Vertex type
|
||||||
*/
|
*/
|
||||||
template<class G, class V, class KEY>
|
template<class G, class V, class KEY>
|
||||||
boost::tuple<G, V, std::map<KEY,V> > predecessorMap2Graph(const PredecessorMap<KEY>& p_map);
|
std::tuple<G, V, std::map<KEY,V> > predecessorMap2Graph(const PredecessorMap<KEY>& p_map);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Compose the poses by following the chain specified by the spanning tree
|
* Compose the poses by following the chain specified by the spanning tree
|
||||||
|
|
|
@ -417,7 +417,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
|
||||||
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
|
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
|
||||||
bool didNotExist;
|
bool didNotExist;
|
||||||
VectorValues::iterator it;
|
VectorValues::iterator it;
|
||||||
boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
|
std::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
|
||||||
if (didNotExist)
|
if (didNotExist)
|
||||||
it->second = alpha * y[i]; // init
|
it->second = alpha * y[i]; // init
|
||||||
else
|
else
|
||||||
|
|
|
@ -102,7 +102,7 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor)
|
||||||
// Do Cholesky to get a Jacobian
|
// Do Cholesky to get a Jacobian
|
||||||
size_t maxrank;
|
size_t maxrank;
|
||||||
bool success;
|
bool success;
|
||||||
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
|
std::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
|
||||||
|
|
||||||
// Check that Cholesky succeeded OR it managed to factor the full Hessian.
|
// Check that Cholesky succeeded OR it managed to factor the full Hessian.
|
||||||
// THe latter case occurs with non-positive definite matrices arising from QP.
|
// THe latter case occurs with non-positive definite matrices arising from QP.
|
||||||
|
@ -122,7 +122,7 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor)
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Helper functions for combine constructor
|
// Helper functions for combine constructor
|
||||||
namespace {
|
namespace {
|
||||||
boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
|
std::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
|
||||||
const FastVector<JacobianFactor::shared_ptr>& factors,
|
const FastVector<JacobianFactor::shared_ptr>& factors,
|
||||||
const FastVector<VariableSlots::const_iterator>& variableSlots) {
|
const FastVector<VariableSlots::const_iterator>& variableSlots) {
|
||||||
gttic(countDims);
|
gttic(countDims);
|
||||||
|
@ -188,7 +188,7 @@ boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return boost::make_tuple(varDims, m, n);
|
return std::make_tuple(varDims, m, n);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -221,7 +221,7 @@ void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
|
||||||
// Count dimensions
|
// Count dimensions
|
||||||
FastVector<DenseIndex> varDims;
|
FastVector<DenseIndex> varDims;
|
||||||
DenseIndex m, n;
|
DenseIndex m, n;
|
||||||
boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots);
|
std::tie(varDims, m, n) = _countDims(jacobians, orderedSlots);
|
||||||
|
|
||||||
// Allocate matrix and copy keys in order
|
// Allocate matrix and copy keys in order
|
||||||
gttic(allocate);
|
gttic(allocate);
|
||||||
|
|
|
@ -480,7 +480,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
|
||||||
const size_t maxRank = min(m, n);
|
const size_t maxRank = min(m, n);
|
||||||
|
|
||||||
// create storage for [R d]
|
// create storage for [R d]
|
||||||
typedef boost::tuple<size_t, Matrix, double> Triple;
|
typedef std::tuple<size_t, Matrix, double> Triple;
|
||||||
list<Triple> Rd;
|
list<Triple> Rd;
|
||||||
|
|
||||||
Matrix rd(1, n + 1); // and for row of R
|
Matrix rd(1, n + 1); // and for row of R
|
||||||
|
@ -506,7 +506,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
|
||||||
rd = Ab.row(*constraint_row);
|
rd = Ab.row(*constraint_row);
|
||||||
|
|
||||||
// Construct solution (r, d, sigma)
|
// Construct solution (r, d, sigma)
|
||||||
Rd.push_back(boost::make_tuple(j, rd, kInfinity));
|
Rd.push_back(std::make_tuple(j, rd, kInfinity));
|
||||||
|
|
||||||
// exit after rank exhausted
|
// exit after rank exhausted
|
||||||
if (Rd.size() >= maxRank)
|
if (Rd.size() >= maxRank)
|
||||||
|
@ -552,7 +552,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
|
||||||
rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j);
|
rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j);
|
||||||
|
|
||||||
// construct solution (r, d, sigma)
|
// construct solution (r, d, sigma)
|
||||||
Rd.push_back(boost::make_tuple(j, rd, precision));
|
Rd.push_back(std::make_tuple(j, rd, precision));
|
||||||
} else {
|
} else {
|
||||||
// If precision is zero, no information on this column
|
// If precision is zero, no information on this column
|
||||||
// This is actually not limited to constraints, could happen in Gaussian::QR
|
// This is actually not limited to constraints, could happen in Gaussian::QR
|
||||||
|
@ -577,9 +577,9 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
|
||||||
bool mixed = false;
|
bool mixed = false;
|
||||||
Ab.setZero(); // make sure we don't look below
|
Ab.setZero(); // make sure we don't look below
|
||||||
for (const Triple& t: Rd) {
|
for (const Triple& t: Rd) {
|
||||||
const size_t& j = t.get<0>();
|
const size_t& j = std::get<0>(t);
|
||||||
const Matrix& rd = t.get<1>();
|
const Matrix& rd = std::get<1>(t);
|
||||||
precisions(i) = t.get<2>();
|
precisions(i) = std::get<2>(t);
|
||||||
if (std::isinf(precisions(i)))
|
if (std::isinf(precisions(i)))
|
||||||
mixed = true;
|
mixed = true;
|
||||||
Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
|
Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
|
||||||
|
|
|
@ -51,7 +51,7 @@ namespace gtsam {
|
||||||
for (const Pair& v : dims) {
|
for (const Pair& v : dims) {
|
||||||
Key key;
|
Key key;
|
||||||
size_t n;
|
size_t n;
|
||||||
boost::tie(key, n) = v;
|
std::tie(key, n) = v;
|
||||||
#ifdef TBB_GREATER_EQUAL_2020
|
#ifdef TBB_GREATER_EQUAL_2020
|
||||||
values_.emplace(key, x.segment(j, n));
|
values_.emplace(key, x.segment(j, n));
|
||||||
#else
|
#else
|
||||||
|
@ -215,11 +215,11 @@ namespace gtsam {
|
||||||
/* ************************************************************************ */
|
/* ************************************************************************ */
|
||||||
namespace internal
|
namespace internal
|
||||||
{
|
{
|
||||||
bool structureCompareOp(const boost::tuple<VectorValues::value_type,
|
bool structureCompareOp(const std::tuple<VectorValues::value_type,
|
||||||
VectorValues::value_type>& vv)
|
VectorValues::value_type>& vv)
|
||||||
{
|
{
|
||||||
return vv.get<0>().first == vv.get<1>().first
|
return std::get<0>(vv).first == std::get<1>(vv).first
|
||||||
&& vv.get<0>().second.size() == vv.get<1>().second.size();
|
&& std::get<0>(vv).second.size() == std::get<1>(vv).second.size();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -236,7 +236,7 @@ namespace gtsam {
|
||||||
if(this->size() != v.size())
|
if(this->size() != v.size())
|
||||||
throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
|
throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
|
||||||
double result = 0.0;
|
double result = 0.0;
|
||||||
typedef boost::tuple<value_type, value_type> ValuePair;
|
typedef std::tuple<value_type, value_type> ValuePair;
|
||||||
using boost::adaptors::map_values;
|
using boost::adaptors::map_values;
|
||||||
for(const ValuePair values: boost::combine(*this, v)) {
|
for(const ValuePair values: boost::combine(*this, v)) {
|
||||||
assert_throw(values.get<0>().first == values.get<1>().first,
|
assert_throw(values.get<0>().first == values.get<1>().first,
|
||||||
|
|
|
@ -52,7 +52,7 @@ static GaussianBayesNet noisyBayesNet = {
|
||||||
TEST( GaussianBayesNet, Matrix )
|
TEST( GaussianBayesNet, Matrix )
|
||||||
{
|
{
|
||||||
Matrix R; Vector d;
|
Matrix R; Vector d;
|
||||||
boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS
|
std::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS
|
||||||
|
|
||||||
Matrix R1 = (Matrix2() <<
|
Matrix R1 = (Matrix2() <<
|
||||||
1.0, 1.0,
|
1.0, 1.0,
|
||||||
|
@ -102,7 +102,7 @@ TEST(GaussianBayesNet, Evaluate2) {
|
||||||
TEST( GaussianBayesNet, NoisyMatrix )
|
TEST( GaussianBayesNet, NoisyMatrix )
|
||||||
{
|
{
|
||||||
Matrix R; Vector d;
|
Matrix R; Vector d;
|
||||||
boost::tie(R,d) = noisyBayesNet.matrix(); // find matrix and RHS
|
std::tie(R,d) = noisyBayesNet.matrix(); // find matrix and RHS
|
||||||
|
|
||||||
Matrix R1 = (Matrix2() <<
|
Matrix R1 = (Matrix2() <<
|
||||||
0.5, 0.5,
|
0.5, 0.5,
|
||||||
|
@ -126,7 +126,7 @@ TEST(GaussianBayesNet, Optimize) {
|
||||||
TEST(GaussianBayesNet, NoisyOptimize) {
|
TEST(GaussianBayesNet, NoisyOptimize) {
|
||||||
Matrix R;
|
Matrix R;
|
||||||
Vector d;
|
Vector d;
|
||||||
boost::tie(R, d) = noisyBayesNet.matrix(); // find matrix and RHS
|
std::tie(R, d) = noisyBayesNet.matrix(); // find matrix and RHS
|
||||||
const Vector x = R.inverse() * d;
|
const Vector x = R.inverse() * d;
|
||||||
const VectorValues expected{{_x_, x.head(1)}, {_y_, x.tail(1)}};
|
const VectorValues expected{{_x_, x.head(1)}, {_y_, x.tail(1)}};
|
||||||
|
|
||||||
|
@ -239,7 +239,7 @@ TEST( GaussianBayesNet, MatrixStress )
|
||||||
const Ordering ordering(keys);
|
const Ordering ordering(keys);
|
||||||
Matrix R;
|
Matrix R;
|
||||||
Vector d;
|
Vector d;
|
||||||
boost::tie(R, d) = bn.matrix(ordering);
|
std::tie(R, d) = bn.matrix(ordering);
|
||||||
EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
|
EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -157,7 +157,7 @@ TEST(GaussianFactorGraph, matrices) {
|
||||||
Vector b = Ab.col(Ab.cols() - 1);
|
Vector b = Ab.col(Ab.cols() - 1);
|
||||||
Matrix actualA;
|
Matrix actualA;
|
||||||
Vector actualb;
|
Vector actualb;
|
||||||
boost::tie(actualA, actualb) = gfg.jacobian();
|
std::tie(actualA, actualb) = gfg.jacobian();
|
||||||
EXPECT(assert_equal(A, actualA));
|
EXPECT(assert_equal(A, actualA));
|
||||||
EXPECT(assert_equal(b, actualb));
|
EXPECT(assert_equal(b, actualb));
|
||||||
|
|
||||||
|
@ -166,7 +166,7 @@ TEST(GaussianFactorGraph, matrices) {
|
||||||
Vector eta = A.transpose() * b;
|
Vector eta = A.transpose() * b;
|
||||||
Matrix actualL;
|
Matrix actualL;
|
||||||
Vector actualeta;
|
Vector actualeta;
|
||||||
boost::tie(actualL, actualeta) = gfg.hessian();
|
std::tie(actualL, actualeta) = gfg.hessian();
|
||||||
EXPECT(assert_equal(L, actualL));
|
EXPECT(assert_equal(L, actualL));
|
||||||
EXPECT(assert_equal(eta, actualeta));
|
EXPECT(assert_equal(eta, actualeta));
|
||||||
|
|
||||||
|
@ -247,7 +247,7 @@ TEST(GaussianFactorGraph, eliminate_empty) {
|
||||||
gfg.add(JacobianFactor());
|
gfg.add(JacobianFactor());
|
||||||
GaussianBayesNet::shared_ptr actualBN;
|
GaussianBayesNet::shared_ptr actualBN;
|
||||||
GaussianFactorGraph::shared_ptr remainingGFG;
|
GaussianFactorGraph::shared_ptr remainingGFG;
|
||||||
boost::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());
|
std::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());
|
||||||
|
|
||||||
// expected Bayes net is empty
|
// expected Bayes net is empty
|
||||||
GaussianBayesNet expectedBN;
|
GaussianBayesNet expectedBN;
|
||||||
|
@ -265,10 +265,10 @@ TEST(GaussianFactorGraph, matrices2) {
|
||||||
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
|
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
|
||||||
Matrix A;
|
Matrix A;
|
||||||
Vector b;
|
Vector b;
|
||||||
boost::tie(A, b) = gfg.jacobian();
|
std::tie(A, b) = gfg.jacobian();
|
||||||
Matrix AtA;
|
Matrix AtA;
|
||||||
Vector eta;
|
Vector eta;
|
||||||
boost::tie(AtA, eta) = gfg.hessian();
|
std::tie(AtA, eta) = gfg.hessian();
|
||||||
EXPECT(assert_equal(A.transpose() * A, AtA));
|
EXPECT(assert_equal(A.transpose() * A, AtA));
|
||||||
EXPECT(assert_equal(A.transpose() * b, eta));
|
EXPECT(assert_equal(A.transpose() * b, eta));
|
||||||
Matrix expectedAtA(6, 6);
|
Matrix expectedAtA(6, 6);
|
||||||
|
@ -316,7 +316,7 @@ TEST(GaussianFactorGraph, multiplyHessianAdd2) {
|
||||||
// brute force
|
// brute force
|
||||||
Matrix AtA;
|
Matrix AtA;
|
||||||
Vector eta;
|
Vector eta;
|
||||||
boost::tie(AtA, eta) = gfg.hessian();
|
std::tie(AtA, eta) = gfg.hessian();
|
||||||
Vector X(6);
|
Vector X(6);
|
||||||
X << 1, 2, 3, 4, 5, 6;
|
X << 1, 2, 3, 4, 5, 6;
|
||||||
Vector Y(6);
|
Vector Y(6);
|
||||||
|
@ -343,10 +343,10 @@ TEST(GaussianFactorGraph, matricesMixed) {
|
||||||
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
|
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
|
||||||
Matrix A;
|
Matrix A;
|
||||||
Vector b;
|
Vector b;
|
||||||
boost::tie(A, b) = gfg.jacobian(); // incorrect !
|
std::tie(A, b) = gfg.jacobian(); // incorrect !
|
||||||
Matrix AtA;
|
Matrix AtA;
|
||||||
Vector eta;
|
Vector eta;
|
||||||
boost::tie(AtA, eta) = gfg.hessian(); // correct
|
std::tie(AtA, eta) = gfg.hessian(); // correct
|
||||||
EXPECT(assert_equal(A.transpose() * A, AtA));
|
EXPECT(assert_equal(A.transpose() * A, AtA));
|
||||||
Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished();
|
Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished();
|
||||||
EXPECT(assert_equal(expected, eta));
|
EXPECT(assert_equal(expected, eta));
|
||||||
|
|
|
@ -295,13 +295,13 @@ TEST(HessianFactor, CombineAndEliminate1) {
|
||||||
Ordering ordering {1};
|
Ordering ordering {1};
|
||||||
GaussianConditional::shared_ptr expectedConditional;
|
GaussianConditional::shared_ptr expectedConditional;
|
||||||
JacobianFactor::shared_ptr expectedFactor;
|
JacobianFactor::shared_ptr expectedFactor;
|
||||||
boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering);
|
std::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering);
|
||||||
CHECK(expectedFactor);
|
CHECK(expectedFactor);
|
||||||
|
|
||||||
// Eliminate
|
// Eliminate
|
||||||
GaussianConditional::shared_ptr actualConditional;
|
GaussianConditional::shared_ptr actualConditional;
|
||||||
HessianFactor::shared_ptr actualHessian;
|
HessianFactor::shared_ptr actualHessian;
|
||||||
boost::tie(actualConditional, actualHessian) = //
|
std::tie(actualConditional, actualHessian) = //
|
||||||
EliminateCholesky(gfg, ordering);
|
EliminateCholesky(gfg, ordering);
|
||||||
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
|
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
|
||||||
|
|
||||||
|
@ -358,13 +358,13 @@ TEST(HessianFactor, CombineAndEliminate2) {
|
||||||
Ordering ordering {0};
|
Ordering ordering {0};
|
||||||
GaussianConditional::shared_ptr expectedConditional;
|
GaussianConditional::shared_ptr expectedConditional;
|
||||||
JacobianFactor::shared_ptr expectedFactor;
|
JacobianFactor::shared_ptr expectedFactor;
|
||||||
boost::tie(expectedConditional, expectedFactor) = //
|
std::tie(expectedConditional, expectedFactor) = //
|
||||||
jacobian.eliminate(ordering);
|
jacobian.eliminate(ordering);
|
||||||
|
|
||||||
// Eliminate
|
// Eliminate
|
||||||
GaussianConditional::shared_ptr actualConditional;
|
GaussianConditional::shared_ptr actualConditional;
|
||||||
HessianFactor::shared_ptr actualHessian;
|
HessianFactor::shared_ptr actualHessian;
|
||||||
boost::tie(actualConditional, actualHessian) = //
|
std::tie(actualConditional, actualHessian) = //
|
||||||
EliminateCholesky(gfg, ordering);
|
EliminateCholesky(gfg, ordering);
|
||||||
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
|
actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison
|
||||||
|
|
||||||
|
@ -498,7 +498,7 @@ TEST(HessianFactor, gradientAtZero)
|
||||||
|
|
||||||
// test gradient at zero
|
// test gradient at zero
|
||||||
VectorValues expectedG{{0, -g1}, {1, -g2}};
|
VectorValues expectedG{{0, -g1}, {1, -g2}};
|
||||||
Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
|
Matrix A; Vector b; std::tie(A,b) = factor.jacobian();
|
||||||
KeyVector keys {0, 1};
|
KeyVector keys {0, 1};
|
||||||
EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
|
EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
|
||||||
VectorValues actualG = factor.gradientAtZero();
|
VectorValues actualG = factor.gradientAtZero();
|
||||||
|
|
|
@ -371,7 +371,7 @@ TEST(JacobianFactor, operators )
|
||||||
EXPECT(assert_equal(expectedX, actualX));
|
EXPECT(assert_equal(expectedX, actualX));
|
||||||
|
|
||||||
// test gradient at zero
|
// test gradient at zero
|
||||||
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
|
Matrix A; Vector b2; std::tie(A,b2) = lf.jacobian();
|
||||||
VectorValues expectedG;
|
VectorValues expectedG;
|
||||||
expectedG.insert(1, Vector2(20,-10));
|
expectedG.insert(1, Vector2(20,-10));
|
||||||
expectedG.insert(2, Vector2(-20, 10));
|
expectedG.insert(2, Vector2(-20, 10));
|
||||||
|
|
|
@ -68,7 +68,7 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
|
||||||
// Possibly correct for sensor pose
|
// Possibly correct for sensor pose
|
||||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||||
if (p().body_P_sensor)
|
if (p().body_P_sensor)
|
||||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||||
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||||
|
|
||||||
// Save current rotation for updating Jacobians
|
// Save current rotation for updating Jacobians
|
||||||
|
|
|
@ -112,7 +112,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
|
||||||
// Possibly correct for sensor pose by converting to body frame
|
// Possibly correct for sensor pose by converting to body frame
|
||||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||||
if (p().body_P_sensor)
|
if (p().body_P_sensor)
|
||||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||||
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||||
|
|
||||||
// Do update
|
// Do update
|
||||||
|
|
|
@ -125,7 +125,7 @@ TEST(GPSData, init) {
|
||||||
// Estimate initial state
|
// Estimate initial state
|
||||||
Pose3 T;
|
Pose3 T;
|
||||||
Vector3 nV;
|
Vector3 nV;
|
||||||
boost::tie(T, nV) = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796);
|
std::tie(T, nV) = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796);
|
||||||
|
|
||||||
// Check values values
|
// Check values values
|
||||||
EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4));
|
EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4));
|
||||||
|
|
|
@ -143,7 +143,7 @@ T Expression<T>::value(const Values& values,
|
||||||
// Call private version that returns derivatives in H
|
// Call private version that returns derivatives in H
|
||||||
KeyVector keys;
|
KeyVector keys;
|
||||||
FastVector<int> dims;
|
FastVector<int> dims;
|
||||||
boost::tie(keys, dims) = keysAndDims();
|
std::tie(keys, dims) = keysAndDims();
|
||||||
return valueAndDerivatives(values, keys, dims, *H);
|
return valueAndDerivatives(values, keys, dims, *H);
|
||||||
} else
|
} else
|
||||||
// no derivatives needed, just return value
|
// no derivatives needed, just return value
|
||||||
|
|
|
@ -180,7 +180,7 @@ protected:
|
||||||
if (keys_.empty()) {
|
if (keys_.empty()) {
|
||||||
// This is the case when called in ExpressionFactor Constructor.
|
// This is the case when called in ExpressionFactor Constructor.
|
||||||
// We then take the keys from the expression in sorted order.
|
// We then take the keys from the expression in sorted order.
|
||||||
boost::tie(keys_, dims_) = expression_.keysAndDims();
|
std::tie(keys_, dims_) = expression_.keysAndDims();
|
||||||
} else {
|
} else {
|
||||||
// This happens with classes derived from BinaryExpressionFactor etc.
|
// This happens with classes derived from BinaryExpressionFactor etc.
|
||||||
// In that case, the keys_ are already defined and we just need to grab
|
// In that case, the keys_ are already defined and we just need to grab
|
||||||
|
|
|
@ -65,7 +65,7 @@ NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOpt
|
||||||
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
|
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
|
||||||
Values newValues;
|
Values newValues;
|
||||||
int dummy;
|
int dummy;
|
||||||
boost::tie(newValues, dummy) = nonlinearConjugateGradient<System, Values>(
|
std::tie(newValues, dummy) = nonlinearConjugateGradient<System, Values>(
|
||||||
System(graph_), state_->values, params_, true /* single iteration */);
|
System(graph_), state_->values, params_, true /* single iteration */);
|
||||||
state_.reset(new State(newValues, graph_.error(newValues), state_->iterations + 1));
|
state_.reset(new State(newValues, graph_.error(newValues), state_->iterations + 1));
|
||||||
|
|
||||||
|
@ -78,7 +78,7 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() {
|
||||||
System system(graph_);
|
System system(graph_);
|
||||||
Values newValues;
|
Values newValues;
|
||||||
int iterations;
|
int iterations;
|
||||||
boost::tie(newValues, iterations) =
|
std::tie(newValues, iterations) =
|
||||||
nonlinearConjugateGradient(system, state_->values, params_, false);
|
nonlinearConjugateGradient(system, state_->values, params_, false);
|
||||||
state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations));
|
state_.reset(new State(std::move(newValues), graph_.error(newValues), iterations));
|
||||||
return state_->values;
|
return state_->values;
|
||||||
|
|
|
@ -145,7 +145,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
|
||||||
* The last parameter is a switch between gradient-descent and conjugate gradient
|
* The last parameter is a switch between gradient-descent and conjugate gradient
|
||||||
*/
|
*/
|
||||||
template<class S, class V>
|
template<class S, class V>
|
||||||
boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
|
std::tuple<V, int> nonlinearConjugateGradient(const S &system,
|
||||||
const V &initial, const NonlinearOptimizerParams ¶ms,
|
const V &initial, const NonlinearOptimizerParams ¶ms,
|
||||||
const bool singleIteration, const bool gradientDescent = false) {
|
const bool singleIteration, const bool gradientDescent = false) {
|
||||||
|
|
||||||
|
@ -160,7 +160,7 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
|
||||||
std::cout << "Exiting, as error = " << currentError << " < "
|
std::cout << "Exiting, as error = " << currentError << " < "
|
||||||
<< params.errorTol << std::endl;
|
<< params.errorTol << std::endl;
|
||||||
}
|
}
|
||||||
return boost::tie(initial, iteration);
|
return std::tie(initial, iteration);
|
||||||
}
|
}
|
||||||
|
|
||||||
V currentValues = initial;
|
V currentValues = initial;
|
||||||
|
@ -218,7 +218,7 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
|
||||||
<< "nonlinearConjugateGradient: Terminating because reached maximum iterations"
|
<< "nonlinearConjugateGradient: Terminating because reached maximum iterations"
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
|
|
||||||
return boost::tie(currentValues, iteration);
|
return std::tie(currentValues, iteration);
|
||||||
}
|
}
|
||||||
|
|
||||||
} // \ namespace gtsam
|
} // \ namespace gtsam
|
||||||
|
|
|
@ -48,7 +48,7 @@ public:
|
||||||
GaussianFactorGraph::shared_ptr fg;
|
GaussianFactorGraph::shared_ptr fg;
|
||||||
KeyVector variables;
|
KeyVector variables;
|
||||||
variables.push_back(pointKey);
|
variables.push_back(pointKey);
|
||||||
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
|
std::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
|
||||||
//fg->print("fg");
|
//fg->print("fg");
|
||||||
|
|
||||||
JacobianFactor::operator=(JacobianFactor(*fg));
|
JacobianFactor::operator=(JacobianFactor(*fg));
|
||||||
|
|
|
@ -94,7 +94,7 @@ TEST(dataSet, load2D) {
|
||||||
const string filename = findExampleDataFile("w100.graph");
|
const string filename = findExampleDataFile("w100.graph");
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
boost::tie(graph, initial) = load2D(filename);
|
std::tie(graph, initial) = load2D(filename);
|
||||||
EXPECT_LONGS_EQUAL(300, graph->size());
|
EXPECT_LONGS_EQUAL(300, graph->size());
|
||||||
EXPECT_LONGS_EQUAL(100, initial->size());
|
EXPECT_LONGS_EQUAL(100, initial->size());
|
||||||
auto model = noiseModel::Unit::Create(3);
|
auto model = noiseModel::Unit::Create(3);
|
||||||
|
@ -139,13 +139,13 @@ TEST(dataSet, load2DVictoriaPark) {
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
|
|
||||||
// Load all
|
// Load all
|
||||||
boost::tie(graph, initial) = load2D(filename);
|
std::tie(graph, initial) = load2D(filename);
|
||||||
EXPECT_LONGS_EQUAL(10608, graph->size());
|
EXPECT_LONGS_EQUAL(10608, graph->size());
|
||||||
EXPECT_LONGS_EQUAL(7120, initial->size());
|
EXPECT_LONGS_EQUAL(7120, initial->size());
|
||||||
|
|
||||||
// Restrict keys
|
// Restrict keys
|
||||||
size_t maxIndex = 5;
|
size_t maxIndex = 5;
|
||||||
boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex);
|
std::tie(graph, initial) = load2D(filename, nullptr, maxIndex);
|
||||||
EXPECT_LONGS_EQUAL(5, graph->size());
|
EXPECT_LONGS_EQUAL(5, graph->size());
|
||||||
EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well
|
EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well
|
||||||
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
|
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
|
||||||
|
@ -221,7 +221,7 @@ TEST(dataSet, readG2o3D) {
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
|
std::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
|
||||||
EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
|
EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
|
||||||
for (size_t j : {0, 1, 2, 3, 4}) {
|
for (size_t j : {0, 1, 2, 3, 4}) {
|
||||||
EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
|
EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
|
||||||
|
@ -235,7 +235,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
|
std::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
Values expectedValues;
|
Values expectedValues;
|
||||||
Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
|
Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
|
||||||
|
@ -329,7 +329,7 @@ TEST(dataSet, readG2o) {
|
||||||
const string g2oFile = findExampleDataFile("pose2example");
|
const string g2oFile = findExampleDataFile("pose2example");
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
|
std::tie(actualGraph, actualValues) = readG2o(g2oFile);
|
||||||
|
|
||||||
auto model = noiseModel::Diagonal::Precisions(
|
auto model = noiseModel::Diagonal::Precisions(
|
||||||
Vector3(44.721360, 44.721360, 30.901699));
|
Vector3(44.721360, 44.721360, 30.901699));
|
||||||
|
@ -356,7 +356,7 @@ TEST(dataSet, readG2oHuber) {
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
bool is3D = false;
|
bool is3D = false;
|
||||||
boost::tie(actualGraph, actualValues) =
|
std::tie(actualGraph, actualValues) =
|
||||||
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
|
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
|
||||||
|
|
||||||
auto baseModel = noiseModel::Diagonal::Precisions(
|
auto baseModel = noiseModel::Diagonal::Precisions(
|
||||||
|
@ -373,7 +373,7 @@ TEST(dataSet, readG2oTukey) {
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
bool is3D = false;
|
bool is3D = false;
|
||||||
boost::tie(actualGraph, actualValues) =
|
std::tie(actualGraph, actualValues) =
|
||||||
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
|
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
|
||||||
|
|
||||||
auto baseModel = noiseModel::Diagonal::Precisions(
|
auto baseModel = noiseModel::Diagonal::Precisions(
|
||||||
|
@ -390,14 +390,14 @@ TEST( dataSet, writeG2o)
|
||||||
const string g2oFile = findExampleDataFile("pose2example");
|
const string g2oFile = findExampleDataFile("pose2example");
|
||||||
NonlinearFactorGraph::shared_ptr expectedGraph;
|
NonlinearFactorGraph::shared_ptr expectedGraph;
|
||||||
Values::shared_ptr expectedValues;
|
Values::shared_ptr expectedValues;
|
||||||
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
|
std::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
|
||||||
|
|
||||||
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
||||||
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
||||||
|
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
|
std::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
|
||||||
EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
|
EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
|
||||||
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
|
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
|
||||||
}
|
}
|
||||||
|
@ -409,14 +409,14 @@ TEST( dataSet, writeG2o3D)
|
||||||
NonlinearFactorGraph::shared_ptr expectedGraph;
|
NonlinearFactorGraph::shared_ptr expectedGraph;
|
||||||
Values::shared_ptr expectedValues;
|
Values::shared_ptr expectedValues;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
|
std::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
||||||
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
||||||
|
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
|
std::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
|
||||||
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
|
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
|
||||||
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
|
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
|
||||||
}
|
}
|
||||||
|
@ -428,14 +428,14 @@ TEST( dataSet, writeG2o3DNonDiagonalNoise)
|
||||||
NonlinearFactorGraph::shared_ptr expectedGraph;
|
NonlinearFactorGraph::shared_ptr expectedGraph;
|
||||||
Values::shared_ptr expectedValues;
|
Values::shared_ptr expectedValues;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
|
std::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
||||||
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
||||||
|
|
||||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||||
Values::shared_ptr actualValues;
|
Values::shared_ptr actualValues;
|
||||||
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
|
std::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
|
||||||
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
|
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
|
||||||
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
|
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,7 +36,7 @@ TEST(InitializePose3, computePoses2D) {
|
||||||
NonlinearFactorGraph::shared_ptr inputGraph;
|
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||||
Values::shared_ptr posesInFile;
|
Values::shared_ptr posesInFile;
|
||||||
bool is3D = false;
|
bool is3D = false;
|
||||||
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
std::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
auto priorModel = noiseModel::Unit::Create(3);
|
auto priorModel = noiseModel::Unit::Create(3);
|
||||||
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
|
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
|
||||||
|
@ -59,7 +59,7 @@ TEST(InitializePose3, computePoses3D) {
|
||||||
NonlinearFactorGraph::shared_ptr inputGraph;
|
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||||
Values::shared_ptr posesInFile;
|
Values::shared_ptr posesInFile;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
std::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
auto priorModel = noiseModel::Unit::Create(6);
|
auto priorModel = noiseModel::Unit::Create(6);
|
||||||
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
|
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
|
||||||
|
|
|
@ -234,7 +234,7 @@ TEST( InitializePose3, orientationsGradient ) {
|
||||||
NonlinearFactorGraph::shared_ptr matlabGraph;
|
NonlinearFactorGraph::shared_ptr matlabGraph;
|
||||||
Values::shared_ptr matlabValues;
|
Values::shared_ptr matlabValues;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D);
|
std::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D);
|
||||||
|
|
||||||
Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
|
Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
|
||||||
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
|
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
|
||||||
|
@ -269,7 +269,7 @@ TEST(InitializePose3, initializePoses) {
|
||||||
NonlinearFactorGraph::shared_ptr inputGraph;
|
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||||
Values::shared_ptr posesInFile;
|
Values::shared_ptr posesInFile;
|
||||||
bool is3D = true;
|
bool is3D = true;
|
||||||
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
std::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
auto priorModel = noiseModel::Unit::Create(6);
|
auto priorModel = noiseModel::Unit::Create(6);
|
||||||
inputGraph->addPrior(0, Pose3(), priorModel);
|
inputGraph->addPrior(0, Pose3(), priorModel);
|
||||||
|
|
|
@ -260,7 +260,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
|
||||||
string inputFile = findExampleDataFile("noisyToyGraph");
|
string inputFile = findExampleDataFile("noisyToyGraph");
|
||||||
NonlinearFactorGraph::shared_ptr g;
|
NonlinearFactorGraph::shared_ptr g;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
boost::tie(g, initial) = readG2o(inputFile);
|
std::tie(g, initial) = readG2o(inputFile);
|
||||||
|
|
||||||
// Add prior on the pose having index (key) = 0
|
// Add prior on the pose having index (key) = 0
|
||||||
NonlinearFactorGraph graphWithPrior = *g;
|
NonlinearFactorGraph graphWithPrior = *g;
|
||||||
|
@ -281,7 +281,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
|
||||||
string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
|
string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
|
||||||
NonlinearFactorGraph::shared_ptr gmatlab;
|
NonlinearFactorGraph::shared_ptr gmatlab;
|
||||||
Values::shared_ptr expected;
|
Values::shared_ptr expected;
|
||||||
boost::tie(gmatlab, expected) = readG2o(matlabFile);
|
std::tie(gmatlab, expected) = readG2o(matlabFile);
|
||||||
|
|
||||||
for(const auto& key_pose: expected->extract<Pose2>()){
|
for(const auto& key_pose: expected->extract<Pose2>()){
|
||||||
const Key& k = key_pose.first;
|
const Key& k = key_pose.first;
|
||||||
|
@ -296,7 +296,7 @@ TEST( Lago, largeGraphNoisy ) {
|
||||||
string inputFile = findExampleDataFile("noisyToyGraph");
|
string inputFile = findExampleDataFile("noisyToyGraph");
|
||||||
NonlinearFactorGraph::shared_ptr g;
|
NonlinearFactorGraph::shared_ptr g;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
boost::tie(g, initial) = readG2o(inputFile);
|
std::tie(g, initial) = readG2o(inputFile);
|
||||||
|
|
||||||
// Add prior on the pose having index (key) = 0
|
// Add prior on the pose having index (key) = 0
|
||||||
NonlinearFactorGraph graphWithPrior = *g;
|
NonlinearFactorGraph graphWithPrior = *g;
|
||||||
|
@ -308,7 +308,7 @@ TEST( Lago, largeGraphNoisy ) {
|
||||||
string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
|
string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
|
||||||
NonlinearFactorGraph::shared_ptr gmatlab;
|
NonlinearFactorGraph::shared_ptr gmatlab;
|
||||||
Values::shared_ptr expected;
|
Values::shared_ptr expected;
|
||||||
boost::tie(gmatlab, expected) = readG2o(matlabFile);
|
std::tie(gmatlab, expected) = readG2o(matlabFile);
|
||||||
|
|
||||||
for(const auto& key_pose: expected->extract<Pose2>()){
|
for(const auto& key_pose: expected->extract<Pose2>()){
|
||||||
const Key& k = key_pose.first;
|
const Key& k = key_pose.first;
|
||||||
|
|
|
@ -77,7 +77,7 @@ TEST(SymbolicFactor, EliminateSymbolic)
|
||||||
|
|
||||||
SymbolicFactor::shared_ptr actualFactor;
|
SymbolicFactor::shared_ptr actualFactor;
|
||||||
SymbolicConditional::shared_ptr actualConditional;
|
SymbolicConditional::shared_ptr actualConditional;
|
||||||
boost::tie(actualConditional, actualFactor) =
|
std::tie(actualConditional, actualFactor) =
|
||||||
EliminateSymbolic(factors, Ordering{0, 1, 2, 3});
|
EliminateSymbolic(factors, Ordering{0, 1, 2, 3});
|
||||||
|
|
||||||
CHECK(assert_equal(expectedConditional, *actualConditional));
|
CHECK(assert_equal(expectedConditional, *actualConditional));
|
||||||
|
|
|
@ -67,7 +67,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) {
|
||||||
|
|
||||||
SymbolicBayesNet::shared_ptr actualBayesNet;
|
SymbolicBayesNet::shared_ptr actualBayesNet;
|
||||||
SymbolicFactorGraph::shared_ptr actualSfg;
|
SymbolicFactorGraph::shared_ptr actualSfg;
|
||||||
boost::tie(actualBayesNet, actualSfg) =
|
std::tie(actualBayesNet, actualSfg) =
|
||||||
simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
|
simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
|
||||||
|
|
||||||
EXPECT(assert_equal(expectedSfg, *actualSfg));
|
EXPECT(assert_equal(expectedSfg, *actualSfg));
|
||||||
|
@ -75,7 +75,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) {
|
||||||
|
|
||||||
SymbolicBayesNet::shared_ptr actualBayesNet2;
|
SymbolicBayesNet::shared_ptr actualBayesNet2;
|
||||||
SymbolicFactorGraph::shared_ptr actualSfg2;
|
SymbolicFactorGraph::shared_ptr actualSfg2;
|
||||||
boost::tie(actualBayesNet2, actualSfg2) =
|
std::tie(actualBayesNet2, actualSfg2) =
|
||||||
simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
|
simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
|
||||||
|
|
||||||
EXPECT(assert_equal(expectedSfg, *actualSfg2));
|
EXPECT(assert_equal(expectedSfg, *actualSfg2));
|
||||||
|
@ -108,7 +108,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
|
||||||
|
|
||||||
SymbolicBayesTree::shared_ptr actualBayesTree;
|
SymbolicBayesTree::shared_ptr actualBayesTree;
|
||||||
SymbolicFactorGraph::shared_ptr actualFactorGraph;
|
SymbolicFactorGraph::shared_ptr actualFactorGraph;
|
||||||
boost::tie(actualBayesTree, actualFactorGraph) =
|
std::tie(actualBayesTree, actualFactorGraph) =
|
||||||
simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5});
|
simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5});
|
||||||
|
|
||||||
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
|
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
|
||||||
|
@ -124,7 +124,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
|
||||||
|
|
||||||
SymbolicBayesTree::shared_ptr actualBayesTree2;
|
SymbolicBayesTree::shared_ptr actualBayesTree2;
|
||||||
SymbolicFactorGraph::shared_ptr actualFactorGraph2;
|
SymbolicFactorGraph::shared_ptr actualFactorGraph2;
|
||||||
boost::tie(actualBayesTree2, actualFactorGraph2) =
|
std::tie(actualBayesTree2, actualFactorGraph2) =
|
||||||
simpleTestGraph2.eliminatePartialMultifrontal(KeyVector{4, 5});
|
simpleTestGraph2.eliminatePartialMultifrontal(KeyVector{4, 5});
|
||||||
|
|
||||||
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
|
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
|
||||||
|
|
|
@ -102,7 +102,7 @@ class LoopyBelief {
|
||||||
}
|
}
|
||||||
if (debug) subGraph.print("------- Subgraph:");
|
if (debug) subGraph.print("------- Subgraph:");
|
||||||
DiscreteFactor::shared_ptr message;
|
DiscreteFactor::shared_ptr message;
|
||||||
boost::tie(dummyCond, message) =
|
std::tie(dummyCond, message) =
|
||||||
EliminateDiscrete(subGraph, Ordering{neighbor});
|
EliminateDiscrete(subGraph, Ordering{neighbor});
|
||||||
// store the new factor into messages
|
// store the new factor into messages
|
||||||
messages.insert(make_pair(neighbor, message));
|
messages.insert(make_pair(neighbor, message));
|
||||||
|
|
|
@ -77,7 +77,7 @@ list<TimedOdometry> readOdometry() {
|
||||||
|
|
||||||
// load the ranges from TD
|
// load the ranges from TD
|
||||||
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||||
typedef boost::tuple<double, size_t, double> RangeTriple;
|
typedef std::tuple<double, size_t, double> RangeTriple;
|
||||||
vector<RangeTriple> readTriples() {
|
vector<RangeTriple> readTriples() {
|
||||||
vector<RangeTriple> triples;
|
vector<RangeTriple> triples;
|
||||||
string tdFile = findExampleDataFile("Plaza1_TD.txt");
|
string tdFile = findExampleDataFile("Plaza1_TD.txt");
|
||||||
|
@ -165,7 +165,7 @@ int main(int argc, char** argv) {
|
||||||
//--------------------------------- odometry loop -----------------------------------------
|
//--------------------------------- odometry loop -----------------------------------------
|
||||||
double t;
|
double t;
|
||||||
Pose2 odometry;
|
Pose2 odometry;
|
||||||
boost::tie(t, odometry) = timedOdometry;
|
std::tie(t, odometry) = timedOdometry;
|
||||||
printf("step %d, time = %g\n",(int)i,t);
|
printf("step %d, time = %g\n",(int)i,t);
|
||||||
|
|
||||||
// add odometry factor
|
// add odometry factor
|
||||||
|
|
|
@ -76,7 +76,7 @@ list<TimedOdometry> readOdometry() {
|
||||||
|
|
||||||
// load the ranges from TD
|
// load the ranges from TD
|
||||||
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||||
typedef boost::tuple<double, size_t, double> RangeTriple;
|
typedef std::tuple<double, size_t, double> RangeTriple;
|
||||||
vector<RangeTriple> readTriples() {
|
vector<RangeTriple> readTriples() {
|
||||||
vector<RangeTriple> triples;
|
vector<RangeTriple> triples;
|
||||||
string tdFile = findExampleDataFile("Plaza2_TD.txt");
|
string tdFile = findExampleDataFile("Plaza2_TD.txt");
|
||||||
|
@ -146,7 +146,7 @@ int main(int argc, char** argv) {
|
||||||
//--------------------------------- odometry loop -----------------------------------------
|
//--------------------------------- odometry loop -----------------------------------------
|
||||||
double t;
|
double t;
|
||||||
Pose2 odometry;
|
Pose2 odometry;
|
||||||
boost::tie(t, odometry) = timedOdometry;
|
std::tie(t, odometry) = timedOdometry;
|
||||||
|
|
||||||
// add odometry factor
|
// add odometry factor
|
||||||
newFactors.push_back(
|
newFactors.push_back(
|
||||||
|
|
|
@ -132,7 +132,7 @@ TEST(InvDepthFactor, backproject)
|
||||||
|
|
||||||
Vector5 actual_vec;
|
Vector5 actual_vec;
|
||||||
double actual_inv;
|
double actual_inv;
|
||||||
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
|
std::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
|
||||||
EXPECT(assert_equal(expected,actual_vec,1e-7));
|
EXPECT(assert_equal(expected,actual_vec,1e-7));
|
||||||
EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
|
EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
|
||||||
}
|
}
|
||||||
|
@ -148,7 +148,7 @@ TEST(InvDepthFactor, backproject2)
|
||||||
|
|
||||||
Vector5 actual_vec;
|
Vector5 actual_vec;
|
||||||
double actual_inv;
|
double actual_inv;
|
||||||
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
|
std::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
|
||||||
EXPECT(assert_equal(expected,actual_vec,1e-7));
|
EXPECT(assert_equal(expected,actual_vec,1e-7));
|
||||||
EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
|
EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
|
||||||
}
|
}
|
||||||
|
|
|
@ -45,7 +45,7 @@ namespace gtsam {
|
||||||
*
|
*
|
||||||
* We want the minimum of all those alphas among all inactive inequality.
|
* We want the minimum of all those alphas among all inactive inequality.
|
||||||
*/
|
*/
|
||||||
Template boost::tuple<double, int> This::computeStepSize(
|
Template std::tuple<double, int> This::computeStepSize(
|
||||||
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||||
const VectorValues& p, const double& maxAlpha) const {
|
const VectorValues& p, const double& maxAlpha) const {
|
||||||
double minAlpha = maxAlpha;
|
double minAlpha = maxAlpha;
|
||||||
|
@ -74,7 +74,7 @@ Template boost::tuple<double, int> This::computeStepSize(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return boost::make_tuple(minAlpha, closestFactorIx);
|
return std::make_tuple(minAlpha, closestFactorIx);
|
||||||
}
|
}
|
||||||
|
|
||||||
/******************************************************************************/
|
/******************************************************************************/
|
||||||
|
@ -222,7 +222,7 @@ Template typename This::State This::iterate(
|
||||||
double alpha;
|
double alpha;
|
||||||
int factorIx;
|
int factorIx;
|
||||||
VectorValues p = newValues - state.values;
|
VectorValues p = newValues - state.values;
|
||||||
boost::tie(alpha, factorIx) = // using 16.41
|
std::tie(alpha, factorIx) = // using 16.41
|
||||||
computeStepSize(state.workingSet, state.values, p, POLICY::maxAlpha);
|
computeStepSize(state.workingSet, state.values, p, POLICY::maxAlpha);
|
||||||
// also add to the working set the one that complains the most
|
// also add to the working set the one that complains the most
|
||||||
InequalityFactorGraph newWorkingSet = state.workingSet;
|
InequalityFactorGraph newWorkingSet = state.workingSet;
|
||||||
|
|
|
@ -114,7 +114,7 @@ protected:
|
||||||
* If there is a blocking constraint, the closest one will be added to the
|
* If there is a blocking constraint, the closest one will be added to the
|
||||||
* working set and become active in the next iteration.
|
* working set and become active in the next iteration.
|
||||||
*/
|
*/
|
||||||
boost::tuple<double, int> computeStepSize(
|
std::tuple<double, int> computeStepSize(
|
||||||
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||||
const VectorValues& p, const double& maxAlpha) const;
|
const VectorValues& p, const double& maxAlpha) const;
|
||||||
|
|
||||||
|
|
|
@ -73,7 +73,7 @@ TEST(LPInitSolver, InfiniteLoopSingleVar) {
|
||||||
VectorValues starter;
|
VectorValues starter;
|
||||||
starter.insert(1, Vector3(0, 0, 2));
|
starter.insert(1, Vector3(0, 0, 2));
|
||||||
VectorValues results, duals;
|
VectorValues results, duals;
|
||||||
boost::tie(results, duals) = solver.optimize(starter);
|
std::tie(results, duals) = solver.optimize(starter);
|
||||||
VectorValues expected;
|
VectorValues expected;
|
||||||
expected.insert(1, Vector3(13.5, 6.5, -6.5));
|
expected.insert(1, Vector3(13.5, 6.5, -6.5));
|
||||||
CHECK(assert_equal(results, expected, 1e-7));
|
CHECK(assert_equal(results, expected, 1e-7));
|
||||||
|
@ -101,7 +101,7 @@ TEST(LPInitSolver, InfiniteLoopMultiVar) {
|
||||||
starter.insert(Y, kZero);
|
starter.insert(Y, kZero);
|
||||||
starter.insert(Z, Vector::Constant(1, 2.0));
|
starter.insert(Z, Vector::Constant(1, 2.0));
|
||||||
VectorValues results, duals;
|
VectorValues results, duals;
|
||||||
boost::tie(results, duals) = solver.optimize(starter);
|
std::tie(results, duals) = solver.optimize(starter);
|
||||||
VectorValues expected;
|
VectorValues expected;
|
||||||
expected.insert(X, Vector::Constant(1, 13.5));
|
expected.insert(X, Vector::Constant(1, 13.5));
|
||||||
expected.insert(Y, Vector::Constant(1, 6.5));
|
expected.insert(Y, Vector::Constant(1, 6.5));
|
||||||
|
@ -201,7 +201,7 @@ TEST(LPSolver, SimpleTest1) {
|
||||||
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
||||||
|
|
||||||
VectorValues result, duals;
|
VectorValues result, duals;
|
||||||
boost::tie(result, duals) = lpSolver.optimize(init);
|
std::tie(result, duals) = lpSolver.optimize(init);
|
||||||
VectorValues expectedResult;
|
VectorValues expectedResult;
|
||||||
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
||||||
CHECK(assert_equal(expectedResult, result, 1e-10));
|
CHECK(assert_equal(expectedResult, result, 1e-10));
|
||||||
|
@ -213,7 +213,7 @@ TEST(LPSolver, TestWithoutInitialValues) {
|
||||||
LPSolver lpSolver(lp);
|
LPSolver lpSolver(lp);
|
||||||
VectorValues result, duals, expectedResult;
|
VectorValues result, duals, expectedResult;
|
||||||
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
||||||
boost::tie(result, duals) = lpSolver.optimize();
|
std::tie(result, duals) = lpSolver.optimize();
|
||||||
CHECK(assert_equal(expectedResult, result));
|
CHECK(assert_equal(expectedResult, result));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -204,7 +204,7 @@ TEST(LinearEquality, operators) {
|
||||||
// test gradient at zero
|
// test gradient at zero
|
||||||
Matrix A;
|
Matrix A;
|
||||||
Vector b2;
|
Vector b2;
|
||||||
boost::tie(A, b2) = lf.jacobian();
|
std::tie(A, b2) = lf.jacobian();
|
||||||
VectorValues expectedG;
|
VectorValues expectedG;
|
||||||
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
|
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
|
||||||
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
|
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
|
||||||
|
|
|
@ -252,7 +252,7 @@ namespace gtsam { namespace partition {
|
||||||
// run ND on the graph
|
// run ND on the graph
|
||||||
size_t sepsize;
|
size_t sepsize;
|
||||||
sharedInts part;
|
sharedInts part;
|
||||||
boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
|
std::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
|
||||||
if (!sepsize) return std::optional<MetisResult>();
|
if (!sepsize) return std::optional<MetisResult>();
|
||||||
|
|
||||||
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
|
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
|
||||||
|
@ -312,7 +312,7 @@ namespace gtsam { namespace partition {
|
||||||
// run metis on the graph
|
// run metis on the graph
|
||||||
int edgecut;
|
int edgecut;
|
||||||
sharedInts part;
|
sharedInts part;
|
||||||
boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
|
std::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
|
||||||
|
|
||||||
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
|
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
|
||||||
MetisResult result;
|
MetisResult result;
|
||||||
|
|
|
@ -22,7 +22,7 @@ namespace gtsam { namespace partition {
|
||||||
fg_(fg), ordering_(ordering){
|
fg_(fg), ordering_(ordering){
|
||||||
GenericUnaryGraph unaryFactors;
|
GenericUnaryGraph unaryFactors;
|
||||||
GenericGraph gfg;
|
GenericGraph gfg;
|
||||||
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
std::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
||||||
|
|
||||||
// build reverse mapping from integer to symbol
|
// build reverse mapping from integer to symbol
|
||||||
int numNodes = ordering.size();
|
int numNodes = ordering.size();
|
||||||
|
@ -46,7 +46,7 @@ namespace gtsam { namespace partition {
|
||||||
const NLG& fg, const Ordering& ordering, const std::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
|
const NLG& fg, const Ordering& ordering, const std::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
|
||||||
GenericUnaryGraph unaryFactors;
|
GenericUnaryGraph unaryFactors;
|
||||||
GenericGraph gfg;
|
GenericGraph gfg;
|
||||||
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
std::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
||||||
|
|
||||||
// build reverse mapping from integer to symbol
|
// build reverse mapping from integer to symbol
|
||||||
int numNodes = ordering.size();
|
int numNodes = ordering.size();
|
||||||
|
|
|
@ -74,7 +74,7 @@ TEST (AHRS, Mechanization_integrate) {
|
||||||
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
||||||
Mechanization_bRn2 mech;
|
Mechanization_bRn2 mech;
|
||||||
KalmanFilter::State state;
|
KalmanFilter::State state;
|
||||||
// boost::tie(mech,state) = ahrs.initialize(g_e);
|
// std::tie(mech,state) = ahrs.initialize(g_e);
|
||||||
// Vector u = Vector3(0.05,0.0,0.0);
|
// Vector u = Vector3(0.05,0.0,0.0);
|
||||||
// double dt = 2;
|
// double dt = 2;
|
||||||
// Rot3 expected;
|
// Rot3 expected;
|
||||||
|
|
|
@ -235,7 +235,7 @@ TEST(ExpressionFactor, Shallow) {
|
||||||
// Get and check keys and dims
|
// Get and check keys and dims
|
||||||
KeyVector keys;
|
KeyVector keys;
|
||||||
FastVector<int> dims;
|
FastVector<int> dims;
|
||||||
boost::tie(keys, dims) = expression.keysAndDims();
|
std::tie(keys, dims) = expression.keysAndDims();
|
||||||
LONGS_EQUAL(2,keys.size());
|
LONGS_EQUAL(2,keys.size());
|
||||||
LONGS_EQUAL(2,dims.size());
|
LONGS_EQUAL(2,dims.size());
|
||||||
LONGS_EQUAL(1,keys[0]);
|
LONGS_EQUAL(1,keys[0]);
|
||||||
|
|
|
@ -117,7 +117,7 @@ TEST(GaussianFactorGraph, eliminateOne_x1_fast) {
|
||||||
GaussianFactorGraph fg = createGaussianFactorGraph();
|
GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||||
GaussianConditional::shared_ptr conditional;
|
GaussianConditional::shared_ptr conditional;
|
||||||
JacobianFactor::shared_ptr remaining;
|
JacobianFactor::shared_ptr remaining;
|
||||||
boost::tie(conditional, remaining) = EliminateQR(fg, Ordering{X(1)});
|
std::tie(conditional, remaining) = EliminateQR(fg, Ordering{X(1)});
|
||||||
|
|
||||||
// create expected Conditional Gaussian
|
// create expected Conditional Gaussian
|
||||||
Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I;
|
Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I;
|
||||||
|
@ -295,7 +295,7 @@ TEST(GaussianFactorGraph, elimination) {
|
||||||
// Check matrix
|
// Check matrix
|
||||||
Matrix R;
|
Matrix R;
|
||||||
Vector d;
|
Vector d;
|
||||||
boost::tie(R, d) = bayesNet.matrix();
|
std::tie(R, d) = bayesNet.matrix();
|
||||||
Matrix expected =
|
Matrix expected =
|
||||||
(Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished();
|
(Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished();
|
||||||
Matrix expected2 =
|
Matrix expected2 =
|
||||||
|
|
|
@ -67,7 +67,7 @@ TEST( GaussianJunctionTreeB, constructor2 ) {
|
||||||
// create a graph
|
// create a graph
|
||||||
NonlinearFactorGraph nlfg;
|
NonlinearFactorGraph nlfg;
|
||||||
Values values;
|
Values values;
|
||||||
boost::tie(nlfg, values) = createNonlinearSmoother(7);
|
std::tie(nlfg, values) = createNonlinearSmoother(7);
|
||||||
SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic();
|
SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic();
|
||||||
|
|
||||||
// linearize
|
// linearize
|
||||||
|
@ -130,7 +130,7 @@ TEST( GaussianJunctionTreeB, constructor2 ) {
|
||||||
// // create a graph
|
// // create a graph
|
||||||
// GaussianFactorGraph fg;
|
// GaussianFactorGraph fg;
|
||||||
// Ordering ordering;
|
// Ordering ordering;
|
||||||
// boost::tie(fg,ordering) = createSmoother(7);
|
// std::tie(fg,ordering) = createSmoother(7);
|
||||||
//
|
//
|
||||||
// // optimize the graph
|
// // optimize the graph
|
||||||
// GaussianJunctionTree tree(fg);
|
// GaussianJunctionTree tree(fg);
|
||||||
|
|
|
@ -739,7 +739,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
|
||||||
const string filename = findExampleDataFile("w100.graph");
|
const string filename = findExampleDataFile("w100.graph");
|
||||||
NonlinearFactorGraph::shared_ptr graph;
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
Values::shared_ptr initial;
|
Values::shared_ptr initial;
|
||||||
boost::tie(graph, initial) = load2D(filename);
|
std::tie(graph, initial) = load2D(filename);
|
||||||
// Add a Gaussian prior on first poses
|
// Add a Gaussian prior on first poses
|
||||||
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
|
||||||
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
|
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
|
||||||
|
|
|
@ -26,7 +26,7 @@ using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
// Generate a small PoseSLAM problem
|
// Generate a small PoseSLAM problem
|
||||||
boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
|
std::tuple<NonlinearFactorGraph, Values> generateProblem() {
|
||||||
|
|
||||||
// 1. Create graph container and add factors to it
|
// 1. Create graph container and add factors to it
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
|
@ -64,7 +64,7 @@ boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
|
||||||
Pose2 x5(2.1, 2.1, -M_PI_2);
|
Pose2 x5(2.1, 2.1, -M_PI_2);
|
||||||
initialEstimate.insert(5, x5);
|
initialEstimate.insert(5, x5);
|
||||||
|
|
||||||
return boost::tie(graph, initialEstimate);
|
return std::tie(graph, initialEstimate);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -73,7 +73,7 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) {
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
Values initialEstimate;
|
Values initialEstimate;
|
||||||
|
|
||||||
boost::tie(graph, initialEstimate) = generateProblem();
|
std::tie(graph, initialEstimate) = generateProblem();
|
||||||
// cout << "initial error = " << graph.error(initialEstimate) << endl;
|
// cout << "initial error = " << graph.error(initialEstimate) << endl;
|
||||||
|
|
||||||
NonlinearOptimizerParams param;
|
NonlinearOptimizerParams param;
|
||||||
|
|
|
@ -68,7 +68,7 @@ TEST( Graph, predecessorMap2Graph )
|
||||||
p_map.insert(1, 2);
|
p_map.insert(1, 2);
|
||||||
p_map.insert(2, 2);
|
p_map.insert(2, 2);
|
||||||
p_map.insert(3, 2);
|
p_map.insert(3, 2);
|
||||||
boost::tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
|
std::tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
|
||||||
|
|
||||||
LONGS_EQUAL(3, (long)boost::num_vertices(graph));
|
LONGS_EQUAL(3, (long)boost::num_vertices(graph));
|
||||||
CHECK(root == key2vertex[2]);
|
CHECK(root == key2vertex[2]);
|
||||||
|
@ -174,7 +174,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree )
|
||||||
// G.push_factor("x3", "x4");
|
// G.push_factor("x3", "x4");
|
||||||
//
|
//
|
||||||
// SymbolicFactorGraph T, C;
|
// SymbolicFactorGraph T, C;
|
||||||
// boost::tie(T, C) = G.splitMinimumSpanningTree();
|
// std::tie(T, C) = G.splitMinimumSpanningTree();
|
||||||
//
|
//
|
||||||
// SymbolicFactorGraph expectedT, expectedC;
|
// SymbolicFactorGraph expectedT, expectedC;
|
||||||
// expectedT.push_factor("x1", "x2");
|
// expectedT.push_factor("x1", "x2");
|
||||||
|
@ -207,7 +207,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree )
|
||||||
//
|
//
|
||||||
// SymbolicFactorGraph singletonGraph;
|
// SymbolicFactorGraph singletonGraph;
|
||||||
// set<Symbol> singletons;
|
// set<Symbol> singletons;
|
||||||
// boost::tie(singletonGraph, singletons) = G.removeSingletons();
|
// std::tie(singletonGraph, singletons) = G.removeSingletons();
|
||||||
//
|
//
|
||||||
// set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
|
// set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
|
||||||
// CHECK(singletons_excepted == singletons);
|
// CHECK(singletons_excepted == singletons);
|
||||||
|
|
|
@ -62,7 +62,7 @@ TEST( Iterative, conjugateGradientDescent )
|
||||||
Matrix A;
|
Matrix A;
|
||||||
Vector b;
|
Vector b;
|
||||||
Vector x0 = Z_6x1;
|
Vector x0 = Z_6x1;
|
||||||
boost::tie(A, b) = fg.jacobian();
|
std::tie(A, b) = fg.jacobian();
|
||||||
Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished();
|
Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished();
|
||||||
|
|
||||||
// Do conjugate gradient descent, System version
|
// Do conjugate gradient descent, System version
|
||||||
|
|
|
@ -67,7 +67,7 @@ TEST(SubgraphPreconditioner, planarGraph) {
|
||||||
// Check planar graph construction
|
// Check planar graph construction
|
||||||
GaussianFactorGraph A;
|
GaussianFactorGraph A;
|
||||||
VectorValues xtrue;
|
VectorValues xtrue;
|
||||||
boost::tie(A, xtrue) = planarGraph(3);
|
std::tie(A, xtrue) = planarGraph(3);
|
||||||
LONGS_EQUAL(13, A.size());
|
LONGS_EQUAL(13, A.size());
|
||||||
LONGS_EQUAL(9, xtrue.size());
|
LONGS_EQUAL(9, xtrue.size());
|
||||||
DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue
|
DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue
|
||||||
|
@ -83,11 +83,11 @@ TEST(SubgraphPreconditioner, splitOffPlanarTree) {
|
||||||
// Build a planar graph
|
// Build a planar graph
|
||||||
GaussianFactorGraph A;
|
GaussianFactorGraph A;
|
||||||
VectorValues xtrue;
|
VectorValues xtrue;
|
||||||
boost::tie(A, xtrue) = planarGraph(3);
|
std::tie(A, xtrue) = planarGraph(3);
|
||||||
|
|
||||||
// Get the spanning tree and constraints, and check their sizes
|
// Get the spanning tree and constraints, and check their sizes
|
||||||
GaussianFactorGraph T, C;
|
GaussianFactorGraph T, C;
|
||||||
boost::tie(T, C) = splitOffPlanarTree(3, A);
|
std::tie(T, C) = splitOffPlanarTree(3, A);
|
||||||
LONGS_EQUAL(9, T.size());
|
LONGS_EQUAL(9, T.size());
|
||||||
LONGS_EQUAL(4, C.size());
|
LONGS_EQUAL(4, C.size());
|
||||||
|
|
||||||
|
@ -103,11 +103,11 @@ TEST(SubgraphPreconditioner, system) {
|
||||||
GaussianFactorGraph Ab;
|
GaussianFactorGraph Ab;
|
||||||
VectorValues xtrue;
|
VectorValues xtrue;
|
||||||
size_t N = 3;
|
size_t N = 3;
|
||||||
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
|
std::tie(Ab, xtrue) = planarGraph(N); // A*x-b
|
||||||
|
|
||||||
// Get the spanning tree and remaining graph
|
// Get the spanning tree and remaining graph
|
||||||
GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
|
GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
|
||||||
boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
|
std::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
|
||||||
|
|
||||||
// Eliminate the spanning tree to build a prior
|
// Eliminate the spanning tree to build a prior
|
||||||
const Ordering ord = planarOrdering(N);
|
const Ordering ord = planarOrdering(N);
|
||||||
|
@ -199,11 +199,11 @@ TEST(SubgraphPreconditioner, conjugateGradients) {
|
||||||
GaussianFactorGraph Ab;
|
GaussianFactorGraph Ab;
|
||||||
VectorValues xtrue;
|
VectorValues xtrue;
|
||||||
size_t N = 3;
|
size_t N = 3;
|
||||||
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
|
std::tie(Ab, xtrue) = planarGraph(N); // A*x-b
|
||||||
|
|
||||||
// Get the spanning tree
|
// Get the spanning tree
|
||||||
GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
|
GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
|
||||||
boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
|
std::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
|
||||||
|
|
||||||
// Eliminate the spanning tree to build a prior
|
// Eliminate the spanning tree to build a prior
|
||||||
GaussianBayesNet Rc1 = *Ab1.eliminateSequential(); // R1*x-c1
|
GaussianBayesNet Rc1 = *Ab1.eliminateSequential(); // R1*x-c1
|
||||||
|
|
|
@ -106,7 +106,7 @@ int main()
|
||||||
JacobianFactor::shared_ptr factor;
|
JacobianFactor::shared_ptr factor;
|
||||||
|
|
||||||
for(int i = 0; i < n; i++)
|
for(int i = 0; i < n; i++)
|
||||||
boost::tie(conditional, factor) =
|
std::tie(conditional, factor) =
|
||||||
JacobianFactor(combined).eliminate(Ordering{_x2_});
|
JacobianFactor(combined).eliminate(Ordering{_x2_});
|
||||||
|
|
||||||
long timeLog2 = clock();
|
long timeLog2 = clock();
|
||||||
|
|
|
@ -63,7 +63,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
|
||||||
//double timePlanarSmootherJoinAug(int N, size_t reps) {
|
//double timePlanarSmootherJoinAug(int N, size_t reps) {
|
||||||
// GaussianFactorGraph fgBase;
|
// GaussianFactorGraph fgBase;
|
||||||
// VectorValues config;
|
// VectorValues config;
|
||||||
// boost::tie(fgBase,config) = planarGraph(N);
|
// std::tie(fgBase,config) = planarGraph(N);
|
||||||
// Ordering ordering = fgBase.getOrdering();
|
// Ordering ordering = fgBase.getOrdering();
|
||||||
// Symbol key = ordering.front();
|
// Symbol key = ordering.front();
|
||||||
//
|
//
|
||||||
|
@ -96,7 +96,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
|
||||||
//double timePlanarSmootherCombined(int N, size_t reps) {
|
//double timePlanarSmootherCombined(int N, size_t reps) {
|
||||||
// GaussianFactorGraph fgBase;
|
// GaussianFactorGraph fgBase;
|
||||||
// VectorValues config;
|
// VectorValues config;
|
||||||
// boost::tie(fgBase,config) = planarGraph(N);
|
// std::tie(fgBase,config) = planarGraph(N);
|
||||||
// Ordering ordering = fgBase.getOrdering();
|
// Ordering ordering = fgBase.getOrdering();
|
||||||
// Symbol key = ordering.front();
|
// Symbol key = ordering.front();
|
||||||
//
|
//
|
||||||
|
|
|
@ -37,7 +37,7 @@ int main(int argc, char *argv[]) {
|
||||||
NonlinearFactorGraph::shared_ptr g;
|
NonlinearFactorGraph::shared_ptr g;
|
||||||
string inputFile = findExampleDataFile("w10000");
|
string inputFile = findExampleDataFile("w10000");
|
||||||
auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
|
auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
|
||||||
boost::tie(g, solution) = load2D(inputFile, model);
|
std::tie(g, solution) = load2D(inputFile, model);
|
||||||
|
|
||||||
// add noise to create initial estimate
|
// add noise to create initial estimate
|
||||||
Values initial;
|
Values initial;
|
||||||
|
|
Loading…
Reference in New Issue