12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/develop' into fix/isam2

release/4.3a0
Frank Dellaert 2018-10-08 23:58:51 -04:00
parent 566315f47d 680a9c4797
commit dbf14ca1d8
11 changed files with 128 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
bitbucket-pipelines.yml
View File

@ -1,9 +1,8 @@
# This is a sample build configuration for C++ – Make.
# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples.
# Only use spaces to indent your .yml configuration.
# Built from sample configuration for C++ – Make.
# Check https://confluence.atlassian.com/x/5Q4SMw for more examples.
# -----
# You can specify a custom docker image from Docker Hub as your build environment.
image: ilssim/cmake-boost-qt5
# Our custom docker image from Docker Hub as the build environment.
image: dellaert/ubuntu-boost-tbb-eigen3:bionic
pipelines:
default:
@ -11,6 +10,6 @@ pipelines:
script: # Modify the commands below to build your repository.
- mkdir build
- cd build
- cmake ..
- make
- make check
- cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF ..
- make -j2
- make -j2 check

18
docker/ubuntu-boost-tbb-eigen3/Dockerfile Normal file
View File

@ -0,0 +1,18 @@
# Get the base Ubuntu image from Docker Hub
FROM ubuntu:bionic
# Update apps on the base image
RUN apt-get -y update && apt-get install -y
# Install C++
RUN apt-get -y install build-essential
# Install boost and cmake
RUN apt-get -y install libboost-all-dev cmake
# Install TBB
RUN apt-get -y install libtbb-dev
# Install latest Eigen
RUN apt-get install -y libeigen3-dev

2
examples/SolverComparer.cpp
View File

@ -206,7 +206,7 @@ int main(int argc, char *argv[]) {
}
#ifdef GTSAM_USE_TBB
std::auto_ptr<tbb::task_scheduler_init> init;
std::unique_ptr<tbb::task_scheduler_init> init;
if(nThreads > 0) {
cout << "Using " << nThreads << " threads" << endl;
init.reset(new tbb::task_scheduler_init(nThreads));

11
gtsam/3rdparty/CCOLAMD/Source/ccolamd.c vendored
View File

@ -1560,8 +1560,9 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */
Int *dead_cols ;
Int set1 ;
Int set2 ;
#ifndef NDEBUG
Int cs ;
#endif
int ok ;
#ifndef NDEBUG
@ -1909,8 +1910,10 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */
p [k] = col ;
ASSERT (A [col] == EMPTY) ;
cs = CMEMBER (col) ;
#ifndef NDEBUG
cs = CMEMBER (col) ;
ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
#endif
A [col] = k ;
k++ ;
@ -1926,11 +1929,11 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */
if (A [col] == EMPTY)
{
k = Col [col].shared2.order ;
cs = CMEMBER (col) ;
#ifndef NDEBUG
cs = CMEMBER (col) ;
dead_cols [cs]-- ;
#endif
ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
#endif
DEBUG1 (("ccolamd output ordering: k "ID" col "ID
" (dense or null col)\n", k, col)) ;
p [k] = col ;

63
gtsam/3rdparty/metis/GKlib/csr.c vendored
View File

@ -1323,55 +1323,52 @@ void gk_csr_Normalize(gk_csr_t *mat, int what, int norm)
ssize_t *ptr;
float *val, sum;
if (what&GK_CSR_ROW && mat->rowval) {
n = mat->nrows;
if (what & GK_CSR_ROW && mat->rowval) {
n = mat->nrows;
ptr = mat->rowptr;
val = mat->rowval;
#pragma omp parallel if (ptr[n] > OMPMINOPS)
#pragma omp parallel if (ptr[n] > OMPMINOPS)
{
#pragma omp for private(j,sum) schedule(static)
for (i=0; i<n; i++) {
for (sum=0.0, j=ptr[i]; j<ptr[i+1]; j++){
if (norm == 2)
sum += val[j]*val[j];
else if (norm == 1)
sum += val[j]; /* assume val[j] > 0 */
#pragma omp for private(j, sum) schedule(static)
for (i = 0; i < n; i++) {
for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++) {
if (norm == 2)
sum += val[j] * val[j];
else if (norm == 1)
sum += val[j]; /* assume val[j] > 0 */
}
if (sum > 0) {
if (norm == 2)
sum=1.0/sqrt(sum);
else if (norm == 1)
sum=1.0/sum;
for (j=ptr[i]; j<ptr[i+1]; j++)
val[j] *= sum;
if (norm == 2)
sum = 1.0 / sqrt(sum);
else if (norm == 1)
sum = 1.0 / sum;
for (j = ptr[i]; j < ptr[i + 1]; j++) val[j] *= sum;
}
}
}
}
if (what&GK_CSR_COL && mat->colval) {
n = mat->ncols;
if (what & GK_CSR_COL && mat->colval) {
n = mat->ncols;
ptr = mat->colptr;
val = mat->colval;
#pragma omp parallel if (ptr[n] > OMPMINOPS)
#pragma omp parallel if (ptr[n] > OMPMINOPS)
{
#pragma omp for private(j,sum) schedule(static)
for (i=0; i<n; i++) {
for (sum=0.0, j=ptr[i]; j<ptr[i+1]; j++)
if (norm == 2)
sum += val[j]*val[j];
else if (norm == 1)
sum += val[j];
#pragma omp for private(j, sum) schedule(static)
for (i = 0; i < n; i++) {
for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++)
if (norm == 2)
sum += val[j] * val[j];
else if (norm == 1)
sum += val[j];
if (sum > 0) {
if (norm == 2)
sum=1.0/sqrt(sum);
else if (norm == 1)
sum=1.0/sum;
for (j=ptr[i]; j<ptr[i+1]; j++)
val[j] *= sum;
if (norm == 2)
sum = 1.0 / sqrt(sum);
else if (norm == 1)
sum = 1.0 / sum;
for (j = ptr[i]; j < ptr[i + 1]; j++) val[j] *= sum;
}
}
}

71
gtsam/3rdparty/metis/GKlib/getopt.c vendored
View File

@ -341,10 +341,10 @@ static int gk_getopt_internal(int argc, char **argv, char *optstring,
if (gk_optind == 0 || !gk_getopt_initialized) {
if (gk_optind == 0)
gk_optind = 1; /* Don't scan ARGV[0], the program name. */
optstring = gk_getopt_initialize (argc, argv, optstring);
gk_getopt_initialized = 1;
}
gk_optind = 1; /* Don't scan ARGV[0], the program name. */
optstring = gk_getopt_initialize(argc, argv, optstring);
gk_getopt_initialized = 1;
}
/* Test whether ARGV[gk_optind] points to a non-option argument.
Either it does not have option syntax, or there is an environment flag
@ -670,37 +670,38 @@ static int gk_getopt_internal(int argc, char **argv, char *optstring,
if (temp[1] == ':') {
if (temp[2] == ':') {
/* This is an option that accepts an argument optionally. */
if (*nextchar != '\0') {
gk_optarg = nextchar;
gk_optind++;
}
else
gk_optarg = NULL;
nextchar = NULL;
}
else {
/* This is an option that requires an argument. */
if (*nextchar != '\0') {
gk_optarg = nextchar;
/* If we end this ARGV-element by taking the rest as an arg, we must advance to the next element now. */
gk_optind++;
}
else if (gk_optind == argc) {
if (print_errors) {
/* 1003.2 specifies the format of this message. */
fprintf(stderr, "%s: option requires an argument -- %c\n", argv[0], c);
}
gk_optopt = c;
if (optstring[0] == ':')
c = ':';
else
c = '?';
}
else
/* We already incremented `gk_optind' once; increment it again when taking next ARGV-elt as argument. */
gk_optarg = argv[gk_optind++];
nextchar = NULL;
/* This is an option that accepts an argument optionally. */
if (*nextchar != '\0') {
gk_optarg = nextchar;
gk_optind++;
} else {
gk_optarg = NULL;
}
nextchar = NULL;
} else {
/* This is an option that requires an argument. */
if (*nextchar != '\0') {
gk_optarg = nextchar;
/* If we end this ARGV-element by taking the rest as an arg, we must
* advance to the next element now. */
gk_optind++;
} else if (gk_optind == argc) {
if (print_errors) {
/* 1003.2 specifies the format of this message. */
fprintf(stderr, "%s: option requires an argument -- %c\n", argv[0],
c);
}
gk_optopt = c;
if (optstring[0] == ':')
c = ':';
else
c = '?';
} else {
/* We already incremented `gk_optind' once; increment it again when
* taking next ARGV-elt as argument. */
gk_optarg = argv[gk_optind++];
}
nextchar = NULL;
}
}
return c;

5
gtsam/base/cholesky.cpp
View File

@ -111,8 +111,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
return true;
assert(ABC.cols() == ABC.rows());
const Eigen::DenseIndex n = ABC.rows() - topleft;
assert(n >= 0 && nFrontal <= size_t(n));
assert(ABC.rows() >= topleft);
const size_t n = static_cast<size_t>(ABC.rows() - topleft);
assert(nFrontal <= size_t(n));
// Create views on blocks
auto A = ABC.block(topleft, topleft, nFrontal, nFrontal);

19
gtsam/geometry/Unit3.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef __clang__
# pragma clang diagnostic push
@ -36,13 +36,14 @@
#include <iostream>
#include <limits>
#include <cmath>
#include <vector>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix3 D_p_point;
Unit3 direction;
@ -54,7 +55,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
/* ************************************************************************* */
Unit3 Unit3::Random(boost::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
// TODO(dellaert): allow any engine without including all of boost :-(
boost::uniform_on_sphere<double> randomDirection(3);
// This variate_generator object is required for versions of boost somewhere
// around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
@ -161,7 +162,8 @@ Matrix3 Unit3::skew() const {
}
/* ************************************************************************* */
double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
OptionalJacobian<1, 2> H_q) const {
// Get the unit vectors of each, and the derivative.
Matrix32 H_pn_p;
Point3 pn = point3(H_p ? &H_pn_p : nullptr);
@ -185,7 +187,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
}
/* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
const Vector2 xi = basis().transpose() * q.p_;
if (H_q) {
@ -195,7 +197,8 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
}
/* ************************************************************************* */
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
OptionalJacobian<2, 2> H_q) const {
// Get the point3 of this, and the derivative.
Matrix32 H_qn_q;
const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);
@ -230,7 +233,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
}
/* ************************************************************************* */
double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
Matrix2 H_xi_q;
const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
const double theta = xi.norm();
@ -277,4 +280,4 @@ Vector2 Unit3::localCoordinates(const Unit3& other) const {
}
/* ************************************************************************* */
}
} // namespace gtsam

12
gtsam/geometry/tests/testPoint2.cpp
View File

@ -100,12 +100,12 @@ TEST( Point2, expmap) {
/* ************************************************************************* */
TEST( Point2, arithmetic) {
EXPECT(assert_equal<Point2>( Point2(-5,-6), -Point2(5,6) ));
EXPECT(assert_equal<Point2>( Point2(5,6), Point2(4,5)+Point2(1,1)));
EXPECT(assert_equal<Point2>( Point2(3,4), Point2(4,5)-Point2(1,1) ));
EXPECT(assert_equal<Point2>( Point2(8,6), Point2(4,3)*2));
EXPECT(assert_equal<Point2>( Point2(4,6), 2*Point2(2,3)));
EXPECT(assert_equal<Point2>( Point2(2,3), Point2(4,6)/2));
EXPECT(assert_equal<Point2>(Point2(-5, -6), -Point2(5, 6)));
EXPECT(assert_equal<Point2>(Point2(5, 6), Point2(4, 5) + Point2(1, 1)));
EXPECT(assert_equal<Point2>(Point2(3, 4), Point2(4, 5) - Point2(1, 1)));
EXPECT(assert_equal<Point2>(Point2(8, 6), Point2(4, 3) * 2));
EXPECT(assert_equal<Point2>(Point2(4, 6), 2.0 * Point2(2, 3)));
EXPECT(assert_equal<Point2>(Point2(2, 3), Point2(4, 6) / 2));
}
/* ************************************************************************* */

14
gtsam/inference/BayesTree-inst.h
View File

@ -83,26 +83,28 @@ namespace gtsam {
std::string parent = out.str();
parent += "[label=\"";
for(Key index: clique->conditional_->frontals()) {
if(!first) parent += ","; first = false;
for (Key index : clique->conditional_->frontals()) {
if (!first) parent += ",";
first = false;
parent += indexFormatter(index);
}
if(clique->parent()){
if (clique->parent()) {
parent += " : ";
s << parentnum << "->" << num << "\n";
}
first = true;
for(Key sep: clique->conditional_->parents()) {
if(!first) parent += ","; first = false;
for (Key sep : clique->conditional_->parents()) {
if (!first) parent += ",";
first = false;
parent += indexFormatter(sep);
}
parent += "\"];\n";
s << parent;
parentnum = num;
for(sharedClique c: clique->children) {
for (sharedClique c : clique->children) {
num++;
saveGraph(s, c, indexFormatter, parentnum);
}

2
wrap/FileWriter.cpp
View File

@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const {
bool file_exists = true;
try {
existing_contents = file_contents(filename_.c_str(), add_header);
} catch (CantOpenFile) {
} catch (const CantOpenFile& e) {
file_exists = false;
}