diff --git a/CppUnitLite/Failure.h b/CppUnitLite/Failure.h index 67ac5ba38..c3aa702e9 100644 --- a/CppUnitLite/Failure.h +++ b/CppUnitLite/Failure.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/CppUnitLite/Test.cpp b/CppUnitLite/Test.cpp index 481fceb07..78995a219 100644 --- a/CppUnitLite/Test.cpp +++ b/CppUnitLite/Test.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,7 +36,7 @@ Test *Test::getNext() const } void Test::setNext(Test *test) -{ +{ next_ = test; } @@ -46,9 +46,9 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri return true; result.addFailure ( Failure ( - name_, + name_, boost::lexical_cast (__FILE__), - __LINE__, + __LINE__, boost::lexical_cast (expected), boost::lexical_cast (actual))); @@ -63,10 +63,10 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes return true; result.addFailure ( Failure ( - name_, + name_, boost::lexical_cast (__FILE__), - __LINE__, - expected, + __LINE__, + expected, actual)); return false; diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 23d1e2573..b1fb0cf08 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,7 +12,7 @@ /////////////////////////////////////////////////////////////////////////////// // // TEST.H -// +// // This file contains the Test class along with the macros which make effective // in the harness. // @@ -66,7 +66,7 @@ protected: virtual ~testGroup##testName##Test () {};\ void run (TestResult& result_);} \ testGroup##testName##Instance; \ - void testGroup##testName##Test::run (TestResult& result_) + void testGroup##testName##Test::run (TestResult& result_) /** * Declare friend in a class to test its private methods diff --git a/CppUnitLite/TestHarness.h b/CppUnitLite/TestHarness.h index 273309aa5..65dc0d3f9 100644 --- a/CppUnitLite/TestHarness.h +++ b/CppUnitLite/TestHarness.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,7 +12,7 @@ /////////////////////////////////////////////////////////////////////////////// // // TESTHARNESS.H -// +// // The primary include file for the framework. // /////////////////////////////////////////////////////////////////////////////// diff --git a/CppUnitLite/TestRegistry.cpp b/CppUnitLite/TestRegistry.cpp index b493e81a6..2a123c88f 100644 --- a/CppUnitLite/TestRegistry.cpp +++ b/CppUnitLite/TestRegistry.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -18,26 +18,26 @@ #include "TestResult.h" #include "TestRegistry.h" -void TestRegistry::addTest (Test *test) +void TestRegistry::addTest (Test *test) { instance ().add (test); } -int TestRegistry::runAllTests (TestResult& result) +int TestRegistry::runAllTests (TestResult& result) { return instance ().run (result); } -TestRegistry& TestRegistry::instance () +TestRegistry& TestRegistry::instance () { static TestRegistry registry; return registry; } -void TestRegistry::add (Test *test) +void TestRegistry::add (Test *test) { if (tests == 0) { test->setNext(0); @@ -52,7 +52,7 @@ void TestRegistry::add (Test *test) } -int TestRegistry::run (TestResult& result) +int TestRegistry::run (TestResult& result) { result.testsStarted (); diff --git a/CppUnitLite/TestRegistry.h b/CppUnitLite/TestRegistry.h index 56db991ad..5c3ebc280 100644 --- a/CppUnitLite/TestRegistry.h +++ b/CppUnitLite/TestRegistry.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,9 +12,9 @@ /////////////////////////////////////////////////////////////////////////////// // // TESTREGISTRY.H -// -// TestRegistry is a singleton collection of all the tests to run in a system. -// +// +// TestRegistry is a singleton collection of all the tests to run in a system. +// /////////////////////////////////////////////////////////////////////////////// #ifndef TESTREGISTRY_H @@ -38,7 +38,7 @@ private: void add (Test *test); int run (TestResult& result); - + Test *tests; Test *lastTest; diff --git a/CppUnitLite/TestResult.cpp b/CppUnitLite/TestResult.cpp index 2519c94a9..594590d9e 100644 --- a/CppUnitLite/TestResult.cpp +++ b/CppUnitLite/TestResult.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -22,12 +22,12 @@ TestResult::TestResult () } -void TestResult::testsStarted () +void TestResult::testsStarted () { } -void TestResult::addFailure (const Failure& failure) +void TestResult::addFailure (const Failure& failure) { if (failure.lineNumber < 0) // allow for no line number fprintf (stdout, "%s%s%s%s\n", @@ -48,7 +48,7 @@ void TestResult::addFailure (const Failure& failure) } -void TestResult::testsEnded () +void TestResult::testsEnded () { if (failureCount > 0) fprintf (stdout, "There were %d failures\n", failureCount); diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h index b64b40d75..3897d2990 100644 --- a/CppUnitLite/TestResult.h +++ b/CppUnitLite/TestResult.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,10 +12,10 @@ /////////////////////////////////////////////////////////////////////////////// // // TESTRESULT.H -// +// // A TestResult is a collection of the history of some test runs. Right now // it just collects failures. -// +// /////////////////////////////////////////////////////////////////////////////// #ifndef TESTRESULT_H @@ -26,12 +26,12 @@ class Failure; class TestResult { public: - TestResult (); + TestResult (); virtual ~TestResult() {}; virtual void testsStarted (); virtual void addFailure (const Failure& failure); virtual void testsEnded (); - + int getFailureCount() {return failureCount;} private: diff --git a/cython/gtsam_eigency/eigency_cpp.h b/cython/gtsam_eigency/eigency_cpp.h index 923ca7c94..ce303182e 100644 --- a/cython/gtsam_eigency/eigency_cpp.h +++ b/cython/gtsam_eigency/eigency_cpp.h @@ -35,7 +35,7 @@ inline PyArrayObject *_ndarray_view(double *data, long rows, long cols, return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); } else { // Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride - // If no stride is given, the cow_stride is set to the number of rows. + // If no stride is given, the cow_stride is set to the number of rows. return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); } } @@ -355,7 +355,7 @@ public: FlattenedMap() : Base(NULL, 0, 0), object_(NULL) {} - + FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0) : Base(data, rows, cols, Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)), @@ -363,7 +363,7 @@ public: } FlattenedMap(PyArrayObject *object) - : Base((Scalar *)((PyArrayObject*)object)->data, + : Base((Scalar *)((PyArrayObject*)object)->data, // : Base(_from_numpy((PyArrayObject*)object), (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0], @@ -390,7 +390,7 @@ public: return *this; } - + operator Base() const { return static_cast(*this); } @@ -429,7 +429,7 @@ public: (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime), object_(NULL) { } - + Map(Scalar *data, long rows, long cols) : Base(data, rows, cols), object_(NULL) {} @@ -456,7 +456,7 @@ public: throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); Py_XINCREF(object_); } - + Map &operator=(const Map &other) { if (other.object_) { new (this) Map(other.object_); @@ -474,7 +474,7 @@ public: MapBase::operator=(other); return *this; } - + operator Base() const { return static_cast(*this); } diff --git a/doc/Code/LocalizationExample2.cpp b/doc/Code/LocalizationExample2.cpp index 7fce69566..d22180314 100644 --- a/doc/Code/LocalizationExample2.cpp +++ b/doc/Code/LocalizationExample2.cpp @@ -1,5 +1,5 @@ // add unary measurement factors, like GPS, on all three poses -noiseModel::Diagonal::shared_ptr unaryNoise = +noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 8b80f2b2a..79a54903e 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -5,7 +5,7 @@ public: UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): NoiseModelFactor1(model, j), mx_(x), my_(y) {} - Vector evaluateError(const Pose2& q, + Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const { if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished(); diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp index ef880384c..6e736e2d7 100644 --- a/doc/Code/OdometryExample.cpp +++ b/doc/Code/OdometryExample.cpp @@ -3,13 +3,13 @@ NonlinearFactorGraph graph; // Add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr priorNoise = +noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, priorMean, priorNoise)); // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr odometryNoise = +noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp index 2e2b41704..1738aabc5 100644 --- a/doc/Code/Pose2SLAMExample.cpp +++ b/doc/Code/Pose2SLAMExample.cpp @@ -1,10 +1,10 @@ NonlinearFactorGraph graph; -noiseModel::Diagonal::shared_ptr priorNoise = +noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // Add odometry factors -noiseModel::Diagonal::shared_ptr model = +noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 204af1fea..e3408b67b 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 64c17c3db..6eb08c12e 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index a8e50e1f1..f04b73f6b 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -91,7 +91,7 @@ int main(int argc, char* argv[]) cout << "initial state:\n" << initial_state.transpose() << "\n\n"; // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z); - Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), + Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), initial_state(4), initial_state(5)); Point3 prior_point(initial_state.head<3>()); Pose3 prior_pose(prior_rotation, prior_point); @@ -102,7 +102,7 @@ int main(int argc, char* argv[]) int correction_count = 0; initial_values.insert(X(correction_count), prior_pose); initial_values.insert(V(correction_count), prior_velocity); - initial_values.insert(B(correction_count), prior_imu_bias); + initial_values.insert(B(correction_count), prior_imu_bias); // Assemble prior noise model and add it the graph. noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m @@ -138,7 +138,7 @@ int main(int argc, char* argv[]) p->biasAccCovariance = bias_acc_cov; // acc bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous p->biasAccOmegaInt = bias_acc_omega_int; - + #ifdef USE_COMBINED imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias); #else @@ -154,7 +154,7 @@ int main(int argc, char* argv[]) double current_position_error = 0.0, current_orientation_error = 0.0; double output_time = 0.0; - double dt = 0.005; // The real system has noise, but here, results are nearly + double dt = 0.005; // The real system has noise, but here, results are nearly // exactly the same, so keeping this for simplicity. // All priors have been set up, now iterate through the data file. @@ -203,8 +203,8 @@ int main(int argc, char* argv[]) *preint_imu); graph->add(imu_factor); imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor(B(correction_count-1), - B(correction_count ), + graph->add(BetweenFactor(B(correction_count-1), + B(correction_count ), zero_bias, bias_noise_model)); #endif @@ -215,7 +215,7 @@ int main(int argc, char* argv[]) gps(2)), // D, correction_noise); graph->add(gps_factor); - + // Now optimize and compare results. prop_state = imu_preintegrated_->predict(prev_state, prev_bias); initial_values.insert(X(correction_count), prop_state.pose()); @@ -250,10 +250,10 @@ int main(int argc, char* argv[]) // display statistics cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n"; - fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(), - gps(0), gps(1), gps(2), + gps(0), gps(1), gps(2), gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); output_time += 1.0; diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 17f921fd1..3c07fd4e3 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 3416eb6b7..76b5098f6 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 3b547e70c..6dc0d9a93 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 7c43c22e2..52638fdca 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 68a3fd7e7..551814a6b 100644 --- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -34,47 +34,47 @@ int main(int argc, char* argv[]) { // Concatenate poses to create trajectory poses.insert( poses.end(), poses2.begin(), poses2.end() ); poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3 - auto points = createPoints(); // std::vector of Point3 + auto points = createPoints(); // std::vector of Point3 // (ground-truth) sensor pose in body frame, further an unknown variable Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - + // The graph ExpressionFactorGraph graph; - + // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); - + // Uncertainty bearing range measurement; auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); - + // Expressions for body-frame at key 0 and sensor-tf Pose3_ x_('x', 0); Pose3_ body_T_sensor_('T', 0); - + // Add a prior on the body-pose - graph.addExpressionFactor(x_, poses[0], poseNoise); - + graph.addExpressionFactor(x_, poses[0], poseNoise); + // Simulated measurements from pose for (size_t i = 0; i < poses.size(); ++i) { auto world_T_sensor = poses[i].compose(body_T_sensor_gt); for (size_t j = 0; j < points.size(); ++j) { - + // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform. auto prediction_ = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); - + // Create a *perfect* measurement auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); - + // Add factor graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise); } - + // and add a between factor to the graph if (i > 0) { // And also we have a *perfect* measurement for the between factor. - graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); + graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); } } @@ -82,12 +82,12 @@ int main(int argc, char* argv[]) { Values initial; Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initial.insert(Symbol('x', i), poses[i].compose(delta)); + initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); - + // Initialize body_T_sensor wrongly (because we do not know!) - initial.insert(Symbol('T',0), Pose3()); + initial.insert(Symbol('T',0), Pose3()); std::cout << "initial error: " << graph.error(initial) << std::endl; Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); diff --git a/examples/SFMdata.h b/examples/SFMdata.h index af1f761ee..5462868c9 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -52,9 +52,9 @@ std::vector createPoints() { /* ************************************************************************* */ std::vector createPoses( const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), - const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), + const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), int steps = 8) { - + // Create the set of ground-truth poses // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center std::vector poses; @@ -63,6 +63,6 @@ std::vector createPoses( for(; i < steps; ++i) { poses.push_back(poses[i-1].compose(delta)); } - + return poses; } \ No newline at end of file diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 1f40d8c60..f70a44eec 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index f3915ce22..4300820d1 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -361,7 +361,7 @@ void runIncremental() const auto& measured = factor->measured(); Rot2 measuredBearing = measured.bearing(); double measuredRange = measured.range(); - newVariables.insert(lmKey, + newVariables.insert(lmKey, pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } } diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 27c10deb3..65ab5422d 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -22,7 +22,7 @@ * -moves forward 1 meter * -takes stereo readings on three landmarks */ - + #include #include #include @@ -40,7 +40,7 @@ int main(int argc, char** argv){ NonlinearFactorGraph graph; Pose3 first_pose; graph.emplace_shared >(1, Pose3()); - + //create factor noise model with 3 sigmas of value 1 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); //create stereo camera calibration object with .2m between cameras diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 80831bd21..00aaeedb0 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -50,7 +50,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; @@ -60,7 +60,7 @@ int main(int argc, char** argv){ //create stereo camera calibration object const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); - + ifstream pose_file(pose_loc.c_str()); cout << "Reading camera poses" << endl; int pose_id; @@ -72,7 +72,7 @@ int main(int argc, char** argv){ } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - + // camera and landmark keys size_t x, l; diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 0461c1869..4ce4e7af4 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -36,7 +36,7 @@ int main(int argc, char** argv) { // Each node takes 1 of 7 possible states denoted by 0-6 in following order: // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" - // "Industry(with PhD)" "Academia" "Deceased"] + // "Industry(with PhD)" "Academia" "Deceased"] const size_t nrStates = 7; // define variables diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index 43cd9b34c..f5338bc67 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index d68cedb74..49b99a5b6 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index a0300adf9..0b3d60fa5 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index bdf0543ff..95e8832e0 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index d73accf11..fdbd96dc8 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 77edd5fd5..210bdcc73 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 417570604..e159ffa87 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 310abcf02..a7491d804 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 7bfdd7ea5..21df90513 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -170,7 +170,7 @@ struct FixedDimension { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Helper class to construct the product manifold of two other manifolds, M1 and M2 -/// Deprecated because of limited usefulness, maximum obfuscation +/// Deprecated because of limited usefulness, maximum obfuscation template class ProductManifold: public std::pair { BOOST_CONCEPT_ASSERT((IsManifold)); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 0c37b405e..705d84d25 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -540,7 +540,7 @@ Matrix cholesky_inverse(const Matrix &A) } /* ************************************************************************* */ -// Semantics: +// Semantics: // if B = inverse_square_root(A), then all of the following are true: // inv(B) * inv(B)' == A // inv(B' * B) == A diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index eb1c1bbcc..166297d5f 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 87ead88f0..458538003 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 8792be0e6..1a31aa284 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index ca13047a8..ff82ff27c 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 1cccf0319..a19fbe176 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 7caf490ef..856b559c3 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp index 9f54b9c97..344081c8a 100644 --- a/gtsam/base/VerticalBlockMatrix.cpp +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 9cd566ecf..92031db2b 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h index 9ee20a596..953537bf7 100644 --- a/gtsam/base/deprecated/LieMatrix.h +++ b/gtsam/base/deprecated/LieMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index 68632addc..50ea9d695 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h index b271275c3..60c8103e2 100644 --- a/gtsam/base/deprecated/LieVector.h +++ b/gtsam/base/deprecated/LieVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,7 +36,7 @@ struct GTSAM_EXPORT LieVector : public Vector { /** initialize from a normal vector */ LieVector(const Vector& v) : Vector(v) {} - + template LieVector(const V& v) : Vector(v) {} diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index ce224496e..820fc7333 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 954d3e86b..e475465de 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index cf32f1c3b..88cea8c01 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index f405bdaf1..976dee697 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 5e18e1495..8c68bf8a0 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index bacb9dd1e..74f5e0d41 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 3c2885c18..76c4fc490 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 88a6fa825..468e842f2 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -275,7 +275,7 @@ TEST(Matrix, stream_read ) { -0.3, -8e-2, 5.1, 9.0, 1.2, 3.4, 4.5, 6.7).finished(); - string matrixAsString = + string matrixAsString = "1.1 2.3 4.2 7.6\n" "-0.3 -8e-2 5.1 9.0\n\r" // Test extra spaces and windows newlines "1.2 \t 3.4 4.5 6.7"; // Test tab as separator diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index de1c07dcf..128576107 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 8a54d5469..c315b9c27 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index ac58fcca7..b33f06045 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index b89e15637..ec80baad3 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index ecc8533c1..2ad9fc1e3 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -355,7 +355,7 @@ namespace gtsam { NodePtr choose(const L& label, size_t index) const { if (label_ == label) return branches_[index]; // choose branch - + // second case, not label of interest, just recurse boost::shared_ptr r(new Choice(label_, branches_.size())); for(const NodePtr& branch: branches_) diff --git a/gtsam/discrete/DiscreteBayesTree.cpp b/gtsam/discrete/DiscreteBayesTree.cpp index c7a81aa60..bed50a470 100644 --- a/gtsam/discrete/DiscreteBayesTree.cpp +++ b/gtsam/discrete/DiscreteBayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DiscreteEliminationTree.cpp b/gtsam/discrete/DiscreteEliminationTree.cpp index df59681c2..a2953644d 100644 --- a/gtsam/discrete/DiscreteEliminationTree.cpp +++ b/gtsam/discrete/DiscreteEliminationTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DiscreteEliminationTree.h b/gtsam/discrete/DiscreteEliminationTree.h index 1ba33006e..1ea8a5695 100644 --- a/gtsam/discrete/DiscreteEliminationTree.h +++ b/gtsam/discrete/DiscreteEliminationTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ namespace gtsam { typedef EliminationTree Base; ///< Base class typedef DiscreteEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/discrete/DiscreteJunctionTree.cpp b/gtsam/discrete/DiscreteJunctionTree.cpp index 0e6e2b73e..dc24860eb 100644 --- a/gtsam/discrete/DiscreteJunctionTree.cpp +++ b/gtsam/discrete/DiscreteJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h index 8c00065d9..f5bc9be1d 100644 --- a/gtsam/discrete/DiscreteJunctionTree.h +++ b/gtsam/discrete/DiscreteJunctionTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -51,7 +51,7 @@ namespace gtsam { typedef JunctionTree Base; ///< Base class typedef DiscreteJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using precomputed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index 2158973a9..c2a188e08 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 54deedfdc..96e47fff0 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6ad7aeb86..747f53fe1 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 22966ee37..414fe6711 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 3a0f56c30..51692dc82 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cyclic.cpp b/gtsam/geometry/Cyclic.cpp index 7242459a6..70580c38f 100644 --- a/gtsam/geometry/Cyclic.cpp +++ b/gtsam/geometry/Cyclic.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 15d8154b8..0b81ff0f9 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 2670a63f7..afc1c19ef 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -220,7 +220,7 @@ private: & boost::serialization::make_nvp("PinholeBase", boost::serialization::base_object(*this)); } - + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 7869704ca..a2896ca8d 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - + * See LICENSE for the license information - + * -------------------------------------------------------------------------- */ /** diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index af9481181..1557a09db 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 4cabe7140..9f3ed35b0 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index eaec62b48..f46f12540 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index d92c5bdd7..6134ae3d4 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index a7e021394..82f26aee2 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index db4f74026..fa5d4ffde 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index cd6f09507..2254d8ac8 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index dd9bc9dbd..e0f3c3424 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 4ccb6075b..ccecb09c3 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 7becfc75f..abcef9e5b 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 88976697a..20631bdd6 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 0421e1e44..843dc6cc1 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index d6d1f3149..7cd27a9da 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index d05356271..7944db6bf 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 9ae8eef13..be159ed58 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index dfef0a9c5..f7ff881eb 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationGeometry.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1fae470f9..b2d8c3be9 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -65,7 +65,7 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Point3 triangulateDLT( const std::vector>& projection_matrices, - const Point2Vector& measurements, + const Point2Vector& measurements, double rank_tol = 1e-9); /** diff --git a/gtsam/global_includes.h b/gtsam/global_includes.h index 9eba7c7fc..c04da2e5c 100644 --- a/gtsam/global_includes.h +++ b/gtsam/global_includes.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 1934c58ed..a69fb9b8c 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -65,7 +65,7 @@ namespace gtsam { /// @name Standard Interface /// @{ - + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; }; diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 33a23056b..9935776a5 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/BayesTree.cpp b/gtsam/inference/BayesTree.cpp index 2fb0227b6..e61c5d5a1 100644 --- a/gtsam/inference/BayesTree.cpp +++ b/gtsam/inference/BayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index ee0f3c7b5..892ac5f31 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -96,7 +96,7 @@ namespace gtsam { /** Root cliques */ typedef FastVector Roots; - + /** Root cliques */ Roots roots_; @@ -190,7 +190,7 @@ namespace gtsam { /// @} /// @name Advanced Interface /// @{ - + /** * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index 39b60079c..9879a582c 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 39c738a19..1d486030c 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -75,7 +75,7 @@ namespace gtsam { bool equals(const This& c, double tol = 1e-9) const; /// @} - + public: /// @name Standard Interface /// @{ @@ -102,10 +102,10 @@ namespace gtsam { /** Iterator pointing to first frontal key. */ typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); } - + /** Iterator pointing past the last frontal key. */ typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; } - + /** Iterator pointing to the first parent key. */ typename FACTOR::const_iterator beginParents() const { return endFrontals(); } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index a8f68ca2e..8084aa75b 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -99,7 +99,7 @@ namespace gtsam { /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. - * + * * Example - Full Cholesky elimination in COLAMD order: * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); @@ -108,12 +108,12 @@ namespace gtsam { * Example - METIS ordering for elimination * \code * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); - * + * * Example - Full QR elimination in specified order: * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); * \endcode - * + * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -130,17 +130,17 @@ namespace gtsam { /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) - * + * * Example - Full Cholesky elimination in COLAMD order: * \code * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); * \endcode - * + * * Example - Full QR elimination in specified order: * \code * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); * \endcode - * + * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -183,7 +183,7 @@ namespace gtsam { const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - + /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index fcea1284e..e4a64c589 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -116,7 +116,7 @@ namespace gtsam { This& operator=(const This& other); /// @} - + public: /// @name Standard Interface /// @{ @@ -142,11 +142,11 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; /// @} - + public: /// @name Advanced Interface /// @{ - + /** Return the set of roots (one for a tree, multiple for a forest) */ const FastVector& roots() const { return roots_; } diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 49f34fb2b..58448edbb 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index d44d82954..b53743f06 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 785b2507d..8ed41826e 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h index 36edb423c..d6a40b539 100644 --- a/gtsam/inference/ISAM.h +++ b/gtsam/inference/ISAM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index c82062246..0d9c35d2c 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6b5bfcc03..8e22202ed 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -27,7 +27,7 @@ namespace gtsam { /** * Character and index key used to refer to variables. Will simply cast to a Key, - * i.e., a large integer. Keys are used to retrieve values from Values, + * i.e., a large integer. Keys are used to retrieve values from Values, * specify what variables factors depend on, etc. */ class GTSAM_EXPORT Symbol { diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 352ea166f..303043f13 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/VariableSlots.cpp b/gtsam/inference/VariableSlots.cpp index d89ad2ced..64883fe49 100644 --- a/gtsam/inference/VariableSlots.cpp +++ b/gtsam/inference/VariableSlots.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 8ebd5c13c..f9297d300 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -35,18 +35,18 @@ namespace gtsam { * factor. In each row-block (factor), some of the column-blocks (variables) * may be empty since factors involving different sets of variables are * interleaved. -* +* * VariableSlots describes the 2D block structure of the combined factor. It * is a map >. The Key is the real * variable index of the combined factor slot. The vector tells, for * each row-block (factor), which column-block (variable slot) from the * component factor appears in this block of the combined factor. -* +* * As an example, if the combined factor contains variables 1, 3, and 5, then * "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to * variable index 3), row-block 2 (also meaning component factor 2), comes from * column-block 0 of component factor 2. -* +* * \nosubgrouping */ class VariableSlots : public FastMap > { diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index de614c2b4..3ea66405b 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index deeb511db..1e172e5c7 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 1e3f4cdfc..cc5dc4b88 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- -* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index b0708e660..93a161ccd 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index 18216453d..b463f4131 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/tests/testVariableSlots.cpp b/gtsam/inference/tests/testVariableSlots.cpp index f61b49bdd..a7a0945e7 100644 --- a/gtsam/inference/tests/testVariableSlots.cpp +++ b/gtsam/inference/tests/testVariableSlots.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testVariableSlots.cpp - * @brief + * @brief * @author Richard Roberts * @date Oct 5, 2010 */ diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 41e2d8c84..42164d540 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index 44d68be83..eb844e04d 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index c9ef922be..25672beaf 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 401583bbf..7cbb1a227 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ namespace gtsam { typedef FactorGraph Base; typedef GaussianBayesNet This; typedef GaussianConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedConditional; /// @name Standard Constructors @@ -78,7 +78,7 @@ namespace gtsam { ///@name Linear Algebra ///@{ - + /** * Return (dense) upper-triangular matrix representation */ @@ -113,7 +113,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$. - * + * * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ VectorValues gradient(const VectorValues& x0) const; @@ -121,7 +121,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also * gradient(const GaussianBayesNet&, const VectorValues&). - * + * * @param [output] g A VectorValues to store the gradient, which must be preallocated, see * allocateVectorValues */ VectorValues gradientAtZero() const; diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 0ceecdfd2..95851b8be 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 1b2ad47e0..fcc73f80e 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -94,7 +94,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$. - * + * * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ VectorValues gradient(const VectorValues& x0) const; @@ -102,7 +102,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also * gradient(const GaussianBayesNet&, const VectorValues&). - * + * * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index 45d7ecc3b..fe5b1e0d7 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d9b75c69f..0f29e608b 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 3e35d5e65..3749dcf01 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 4b4fc1665..03ffe9326 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianEliminationTree.cpp b/gtsam/linear/GaussianEliminationTree.cpp index 0daaf0707..87469c8c8 100644 --- a/gtsam/linear/GaussianEliminationTree.cpp +++ b/gtsam/linear/GaussianEliminationTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianEliminationTree.h b/gtsam/linear/GaussianEliminationTree.h index 1a4ba7868..44bd099e9 100644 --- a/gtsam/linear/GaussianEliminationTree.h +++ b/gtsam/linear/GaussianEliminationTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ namespace gtsam { typedef EliminationTree Base; ///< Base class typedef GaussianEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8c72ee734..a80b4dfd4 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -44,7 +44,7 @@ namespace gtsam { /** Default constructor creates empty factor */ GaussianFactor() {} - + /** Construct from container of keys. This constructor is used internally from derived factor * constructors, either from a container of keys or from a boost::assign::list_of. */ template diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index ddbc5c1b3..aea12e8ee 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index 24cf2a95d..55208d4d1 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index b038e5452..016811e83 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index c0d294adf..8a1b145c3 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 7db29d861..65fac877a 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 39abe1b56..90ca6d244 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -140,7 +140,7 @@ namespace gtsam { /** Check whether a variable with key \c j exists. */ bool exists(Key j) const { return find(j) != end(); } - /** + /** * Read/write access to the vector value with key \c j, throws * std::out_of_range if \c j does not exist, identical to operator[](Key). */ @@ -153,7 +153,7 @@ namespace gtsam { return item->second; } - /** + /** * Access the vector value with key \c j (const version), throws * std::out_of_range if \c j does not exist, identical to operator[](Key). */ @@ -220,15 +220,15 @@ namespace gtsam { iterator end() { return values_.end(); } ///< Iterator over variables const_iterator end() const { return values_.end(); } ///< Iterator over variables - /** + /** * Return the iterator corresponding to the requested key, or end() if no - * variable is present with this key. + * variable is present with this key. */ iterator find(Key j) { return values_.find(j); } - /** + /** * Return the iterator corresponding to the requested key, or end() if no - * variable is present with this key. + * variable is present with this key. */ const_iterator find(Key j) const { return values_.find(j); } diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index e61b67f73..58ef7d733 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 07d4de865..08d1fb5b9 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index ac8c2d7c7..35f3d4c72 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 9c8eb468d..74eef9a2c 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 13601844c..47565ed5a 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -120,7 +120,7 @@ TEST( GaussianBayesNet, optimizeIncomplete ) TEST( GaussianBayesNet, optimize3 ) { // y = R*x, x=inv(R)*y - // 4 = 1 1 -1 + // 4 = 1 1 -1 // 5 1 5 // NOTE: we are supplying a new RHS here diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 60387694e..f11f17d57 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 081b1d3d1..6d28ad736 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index c4da59496..5e40aa4a7 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 20fe3ac40..c5b3dab37 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationLinear.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 089747dc6..3c6af9332 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 23cd5c477..19c1f8139 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e0792f873..9926d207a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -151,7 +151,7 @@ class GTSAM_EXPORT PreintegrationBase { /// @name Main functionality /// @{ - /** + /** * Subtract estimate and correct for sensor pose * Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) * Ignore D_correctedOmega_measuredAcc as it is trivially zero @@ -190,8 +190,8 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const; - /** - * Compute errors w.r.t. preintegrated measurements and jacobians + /** + * Compute errors w.r.t. preintegrated measurements and jacobians * wrt pose_i, vel_i, bias_i, pose_j, bias_j */ Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 2c0811ae6..11945e53a 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -139,7 +139,7 @@ private: ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); } - + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 97326f06c..654c1ad33 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -11,7 +11,7 @@ /** * @file DoglegOptimizer.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h index 665f887e2..6c09066c2 100644 --- a/gtsam/nonlinear/ExpressionFactorGraph.h +++ b/gtsam/nonlinear/ExpressionFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 3e75fc23d..a0e7aee78 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -11,7 +11,7 @@ /** * @file GaussNewtonOptimizer.h - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 04bd3d3eb..697fff0b3 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -274,7 +274,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @param newTheta Initial values for new variables * @param theta Current solution to be augmented with new initialization * @param delta Current linear delta to be augmented with zeros - * @param deltaNewton + * @param deltaNewton * @param RgProd * @param keyFormatter Formatter for printing nonlinear keys during debugging */ diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index c6aa52ab2..4d928482e 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index c705e3c8f..5c89425c8 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 4cddc7dda..cd0dd8139 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 88fc0f850..3082b9097 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index caac14bf7..0ccac6cc7 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 16f8eba16..1048cd73a 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -415,7 +415,7 @@ namespace gtsam { static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { return ConstKeyValuePair(key_value.first, *key_value.second); } - + static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { return KeyValuePair(key_value.first, *key_value.second); } diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 5ce7fa7b3..634736800 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index e58c40203..8bdc0652e 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -127,7 +127,7 @@ private: /** * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific - * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord * interface. It is used to keep the testCallRecord unit test in sync. */ const int CallRecordMaxVirtualStaticRows = 5; diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h index c99f05b7d..95fc6d022 100644 --- a/gtsam/nonlinear/internal/JacobianMap.h +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h index 413610f82..8b32b6fdc 100644 --- a/gtsam/nonlinear/nonlinearExceptions.h +++ b/gtsam/nonlinear/nonlinearExceptions.h @@ -41,7 +41,7 @@ namespace gtsam { Key key() const { return key_; } virtual const char* what() const throw() { if(what_.empty()) - what_ = + what_ = "\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\ is not a leaf. To make the variables you would like to marginalize be leaves,\n\ their ordering should be constrained using the constrainedKeys argument to\n\ diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 5fc4e208d..494a59fff 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index 731f69816..c2b245780 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 6dc3e3d2f..90ebcbbba 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationNonlinear.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index b3c557b32..b8eee540d 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp index 1cd042428..877c9adbc 100644 --- a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp +++ b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index b3fd76c67..b52e115fd 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 1a1fac922..7383690f4 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 89291fac5..ea407a4ca 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -122,7 +122,7 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } - + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; public: diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 44a5ee5f2..f6561807e 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 44f317a49..78030ff34 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -2,7 +2,7 @@ * @file PoseRotationPrior.h * * @brief Implements a prior on the rotation component of a pose - * + * * @date Jun 14, 2012 * @author Alex Cunningham */ diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 0b1c0cf63..c38d13b58 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -2,7 +2,7 @@ * @file PoseTranslationPrior.h * * @brief Implements a prior on the translation component of a pose - * + * * @date Jun 14, 2012 * @author Alex Cunningham */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 3c5c42ccc..e2fa04a93 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -104,7 +104,7 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); } - + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index d50674d75..3fa5c1a21 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 68b42f210..a1a267116 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 1e8b330c3..d995bf8e7 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,6 +1,6 @@ /** * @file testBetweenFactor.cpp - * @brief + * @brief * @author Duy-Nguyen Ta * @date Aug 2, 2013 */ diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index b2150dd86..16197ab03 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 1c8f3b8ed..63c50e34b 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 24b818835..9cb55d82f 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp index 038ccecbf..5bc20ad12 100644 --- a/gtsam/symbolic/SymbolicBayesNet.cpp +++ b/gtsam/symbolic/SymbolicBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 4db265036..ab89a4dba 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ namespace gtsam { typedef FactorGraph Base; typedef SymbolicBayesNet This; typedef SymbolicConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedConditional; /// @name Standard Constructors @@ -54,22 +54,22 @@ namespace gtsam { /** Implicit copy/downcast constructor to override explicit template container constructor */ template SymbolicBayesNet(const FactorGraph& graph) : Base(graph) {} - + /// @} /// @name Testable /// @{ - + /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; /// @} - + /// @name Standard Interface /// @{ - + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - + /// @} private: diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp index aab626837..07cb8a012 100644 --- a/gtsam/symbolic/SymbolicBayesTree.cpp +++ b/gtsam/symbolic/SymbolicBayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index c4f09c71c..e28f28764 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicEliminationTree.cpp b/gtsam/symbolic/SymbolicEliminationTree.cpp index bd11274bd..7a2e9b831 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.cpp +++ b/gtsam/symbolic/SymbolicEliminationTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index e20fb67bd..5ec886653 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ namespace gtsam { typedef EliminationTree Base; ///< Base class typedef SymbolicEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree * @param structure The set of factors involving each variable. If this is not precomputed, diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index aca1ba043..66657aa7d 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -89,11 +89,11 @@ namespace gtsam { /// @name Testable /// @{ - + bool equals(const This& other, double tol = 1e-9) const; /// @} - + /// @name Advanced Constructors /// @{ public: @@ -129,7 +129,7 @@ namespace gtsam { /// @name Standard Interface /// @{ - + /** Whether the factor is empty (involves zero variables). */ bool empty() const { return keys_.empty(); } diff --git a/gtsam/symbolic/SymbolicFactorGraph.cpp b/gtsam/symbolic/SymbolicFactorGraph.cpp index 574268472..2824a799c 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.cpp +++ b/gtsam/symbolic/SymbolicFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 0a5427ba3..b6f0de2ae 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -82,10 +82,10 @@ namespace gtsam { SymbolicFactorGraph(const FactorGraph& graph) : Base(graph) {} /// @} - + /// @name Testable /// @{ - + bool equals(const This& fg, double tol = 1e-9) const; /// @} diff --git a/gtsam/symbolic/SymbolicISAM.cpp b/gtsam/symbolic/SymbolicISAM.cpp index d7af82e5c..ff724878b 100644 --- a/gtsam/symbolic/SymbolicISAM.cpp +++ b/gtsam/symbolic/SymbolicISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicISAM.h b/gtsam/symbolic/SymbolicISAM.h index 3f85facbf..874acd582 100644 --- a/gtsam/symbolic/SymbolicISAM.h +++ b/gtsam/symbolic/SymbolicISAM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp index 261f682b0..b26b02d84 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.cpp +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h index fdc5450b3..7a152e532 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.h +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -51,7 +51,7 @@ namespace gtsam { typedef JunctionTree Base; ///< Base class typedef SymbolicJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/symbolic/tests/testSerializationSymbolic.cpp b/gtsam/symbolic/tests/testSerializationSymbolic.cpp index 64b06deec..e6f00b087 100644 --- a/gtsam/symbolic/tests/testSerializationSymbolic.cpp +++ b/gtsam/symbolic/tests/testSerializationSymbolic.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationInference.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 309b8f22e..a92d66f68 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 1b84e291f..fdc28c5c8 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -612,7 +612,7 @@ TEST(SymbolicBayesTree, complicatedMarginal) SymbolicBayesTreeClique::shared_ptr cur; SymbolicBayesTreeClique::shared_ptr root = MakeClique(list_of(11)(12), 2); cur = root; - + root->children += MakeClique(list_of(9)(10)(11)(12), 2); root->children.back()->parent_ = root; diff --git a/gtsam/symbolic/tests/testSymbolicConditional.cpp b/gtsam/symbolic/tests/testSymbolicConditional.cpp index ddc602443..963d0dfef 100644 --- a/gtsam/symbolic/tests/testSymbolicConditional.cpp +++ b/gtsam/symbolic/tests/testSymbolicConditional.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index d22703b14..78bb2182c 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -11,7 +11,7 @@ /** * @file testSymbolicEliminationTree.cpp - * @brief + * @brief * @author Richard Roberts * @date Oct 14, 2010 */ @@ -145,7 +145,7 @@ TEST(EliminationTree, Create2) Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); SymbolicEliminationTree actual(graph, order); - + EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 4e2a7b8c9..378e780cd 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index 994e9b107..e3ab36c94 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index f2d891827..c5b1f4ff1 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp index 56ce1f9a5..3d42c0c1e 100644 --- a/gtsam/symbolic/tests/testVariableIndex.cpp +++ b/gtsam/symbolic/tests/testVariableIndex.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testVariableIndex.cpp - * @brief + * @brief * @author Richard Roberts * @date Sep 26, 2010 */ diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index f544366b2..fd675d0d5 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -244,7 +244,7 @@ namespace gtsam { else if (key < k) node = node->right.root_.get(); else return node->value(); } - + throw std::invalid_argument("BTree::find: key not found"); } diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index ba545de35..c7b8cd417 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam_unstable/base/DSFMap.h index 6ddb74cfd..f5a5ca2b1 100644 --- a/gtsam_unstable/base/DSFMap.h +++ b/gtsam_unstable/base/DSFMap.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index ff9f3a8a6..14b4b4854 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h index dd3668087..b1fff90ef 100644 --- a/gtsam_unstable/base/FixedVector.h +++ b/gtsam_unstable/base/FixedVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/dllexport.h b/gtsam_unstable/base/dllexport.h index f49199c70..903996db4 100644 --- a/gtsam_unstable/base/dllexport.h +++ b/gtsam_unstable/base/dllexport.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testBTree.cpp b/gtsam_unstable/base/tests/testBTree.cpp index bbc22c8e5..fcdbd0393 100644 --- a/gtsam_unstable/base/tests/testBTree.cpp +++ b/gtsam_unstable/base/tests/testBTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp index 298d439bc..c3a59aada 100644 --- a/gtsam_unstable/base/tests/testDSF.cpp +++ b/gtsam_unstable/base/tests/testDSF.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam_unstable/base/tests/testDSFMap.cpp index 9c9143a15..94955e1d9 100644 --- a/gtsam_unstable/base/tests/testDSFMap.cpp +++ b/gtsam_unstable/base/tests/testDSFMap.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -128,14 +128,14 @@ TEST(DSFMap, sets){ set s1, s2; s1 += 1,2,3; s2 += 4,5,6; - + /*for(key_pair st: sets){ cout << "Set " << st.first << " :{"; for(const size_t s: st.second) cout << s << ", "; cout << "}" << endl; }*/ - + EXPECT(s1 == sets[1]); EXPECT(s2 == sets[4]); } diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index 1bae52a80..5c4ca4df3 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index d2efc3a2d..ab21a1924 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -1,6 +1,6 @@ /** * @file testLoopyBelief.cpp - * @brief + * @brief * @author Duy-Nguyen Ta * @date Oct 11, 2013 */ diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index fe1c83ce3..c930a1505 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -46,7 +46,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration00s.txt"); string pose_loc = findExampleDataFile("VO_camera_poses00s.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors00s.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; @@ -57,7 +57,7 @@ int main(int argc, char** argv){ //create stereo camera calibration object const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0)); const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10)); - + initial_estimate.insert(Symbol('K', 0), *noisy_K); noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); @@ -75,7 +75,7 @@ int main(int argc, char** argv){ } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - + noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 8df720cd1..a6a75606b 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 931e85c1a..4d9304f1a 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index c8023b23c..2426ca201 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b cout << "Reading calibration info" << endl; @@ -97,7 +97,7 @@ int main(int argc, char** argv){ Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } - + // camera and landmark keys size_t x, l; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d2fbd7579..03b772a4f 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -1,415 +1,415 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testSimilarity3.cpp - * @brief Unit tests for Similarity3 class - * @author Paul Drews - * @author Zhaoyang Lv - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -using namespace gtsam; -using namespace std; -using symbol_shorthand::X; - -GTSAM_CONCEPT_TESTABLE_INST(Similarity3) - -static const Point3 P(0.2, 0.7, -2); -static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); -static const double s = 4; -static const Similarity3 id; -static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); -static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), - Point3(3.5, -8.2, 4.2), 1); -static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); -static const Similarity3 T4(R, P, s); -static const Similarity3 T5(R, P, 10); -static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform - -//****************************************************************************** -TEST(Similarity3, Concepts) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(Similarity3, Constructors) { - Similarity3 sim3_Construct1; - Similarity3 sim3_Construct2(s); - Similarity3 sim3_Construct3(R, P, s); - Similarity3 sim4_Construct4(R.matrix(), P, s); -} - -//****************************************************************************** -TEST(Similarity3, Getters) { - Similarity3 sim3_default; - EXPECT(assert_equal(Rot3(), sim3_default.rotation())); - EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); - EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); - - Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); - EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation())); - EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation())); - EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); -} - -//****************************************************************************** -TEST(Similarity3, AdjointMap) { - const Matrix4 T = T2.matrix(); - // Check Ad with actual definition - Vector7 delta; - delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; - Matrix4 W = Similarity3::wedge(delta); - Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); - EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); -} - -//****************************************************************************** -TEST(Similarity3, inverse) { - Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); - Matrix3 Re; // some values from matlab - Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120; - Vector3 te(-9.8472, 59.7640, 10.2125); - Similarity3 expected(Re, te, 1.0 / 7.0); - EXPECT(assert_equal(expected, sim3.inverse(), 1e-4)); - EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8)); - - // test lie group inverse - Matrix H1, H2; - EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4)); - EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8)); -} - -//****************************************************************************** -TEST(Similarity3, Multiplication) { - Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); - Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11); - Matrix3 re; - re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530; - Vector3 te(-13.6797, 3.2441, -5.7794); - Similarity3 expected(re, te, 77); - EXPECT(assert_equal(expected, test1 * test2, 1e-2)); -} - -//****************************************************************************** -TEST(Similarity3, Manifold) { - EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); - Vector z = Vector7::Zero(); - Similarity3 sim; - EXPECT(sim.retract(z) == sim); - - Vector7 v = Vector7::Zero(); - v(6) = 2; - Similarity3 sim2; - EXPECT(sim2.retract(z) == sim2); - - EXPECT(assert_equal(z, sim2.localCoordinates(sim))); - - Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); - Vector v3(7); - v3 << 0, 0, 0, 1, 2, 3, 0; - EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); - - Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1); - - Vector vlocal = sim.localCoordinates(other); - - EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); - - Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1); - Rot3 R = Rot3::Rodrigues(0.3, 0, 0); - - Vector vlocal2 = sim.localCoordinates(other2); - - EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); - - // TODO add unit tests for retract and localCoordinates -} - -//****************************************************************************** -TEST( Similarity3, retract_first_order) { - Similarity3 id; - Vector v = Z_7x1; - v(0) = 0.3; - EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); -// v(3) = 0.2; -// v(4) = 0.7; -// v(5) = -2; -// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); -} - -//****************************************************************************** -TEST(Similarity3, localCoordinates_first_order) { - Vector7 d12 = Vector7::Constant(0.1); - d12(6) = 1.0; - Similarity3 t1 = T1, t2 = t1.retract(d12); - EXPECT(assert_equal(d12, t1.localCoordinates(t2))); -} - -//****************************************************************************** -TEST(Similarity3, manifold_first_order) { - Similarity3 t1 = T1; - Similarity3 t2 = T3; - Similarity3 origin; - Vector d12 = t1.localCoordinates(t2); - EXPECT(assert_equal(t2, t1.retract(d12))); - Vector d21 = t2.localCoordinates(t1); - EXPECT(assert_equal(t1, t2.retract(d21))); -} - -//****************************************************************************** -// Return as a 4*4 Matrix -TEST(Similarity3, Matrix) { - Matrix4 expected; - expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5; - Matrix4 actual = T6.matrix(); - EXPECT(assert_equal(expected, actual)); -} - -//***************************************************************************** -// Exponential and log maps -TEST(Similarity3, ExpLogMap) { - Vector7 delta; - delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; - Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); - EXPECT(assert_equal(delta, actual)); - - Vector7 zeros; - zeros << 0, 0, 0, 0, 0, 0, 0; - Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); - EXPECT(assert_equal(zeros, logIdentity)); - - Similarity3 expZero = Similarity3::Expmap(zeros); - Similarity3 ident = Similarity3::identity(); - EXPECT(assert_equal(expZero, ident)); - - // Compare to matrix exponential, using expm in Lie.h - EXPECT( - assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); -} - -//****************************************************************************** -// Group action on Point3 (with simpler transform) -TEST(Similarity3, GroupAction) { - EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0))); - EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0))); - - // Test group action on R^4 via matrix representation - Vector4 qh; - qh << 1, 0, 0, 1; - Vector4 ph; - ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0) - EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); - - // Test some more... - Point3 pa = Point3(1, 0, 0); - Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); - Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); - EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); - EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); - - Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0); - Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0); - EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); - EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); - - // Test derivative - boost::function f = boost::bind( - &Similarity3::transform_from, _1, _2, boost::none, boost::none); - - Point3 q(1, 2, 3); - for (const auto T : { T1, T2, T3, T4, T5, T6 }) { - Point3 q(1, 0, 0); - Matrix H1 = numericalDerivative21(f, T, q); - Matrix H2 = numericalDerivative22(f, T, q); - Matrix actualH1, actualH2; - T.transform_from(q, actualH1, actualH2); - EXPECT(assert_equal(H1, actualH1)); - EXPECT(assert_equal(H2, actualH2)); - } -} - -//****************************************************************************** -// Test very simple prior optimization example -TEST(Similarity3, Optimization) { - // Create a PriorFactor with a Sim3 prior - Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); - Symbol key('x', 1); - PriorFactor factor(key, prior, model); - - // Create graph - NonlinearFactorGraph graph; - graph.push_back(factor); - - // Create initial estimate with identity transform - Values initial; - initial.insert(key, Similarity3()); - - // Optimize - Values result; - LevenbergMarquardtParams params; - params.setVerbosityLM("TRYCONFIG"); - result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - - // After optimization, result should be prior - EXPECT(assert_equal(prior, result.at(key), 1e-4)); -} - -//****************************************************************************** -// Test optimization with both Prior and BetweenFactors -TEST(Similarity3, Optimization2) { - Similarity3 prior = Similarity3(); - Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0), - 1.0); - Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), - Point3(sqrt(8) * 0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0), - Point3(sqrt(32) * 0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), - Point3(6 * 0.7, 0, 0), 1.0); - Similarity3 loop = Similarity3(1.42); - - //prior.print("Goal Transform"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, - 0.01); - SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); - SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); - PriorFactor factor(X(1), prior, model); // Prior ! - BetweenFactor b1(X(1), X(2), m1, betweenNoise); - BetweenFactor b2(X(2), X(3), m2, betweenNoise); - BetweenFactor b3(X(3), X(4), m3, betweenNoise); - BetweenFactor b4(X(4), X(5), m4, betweenNoise); - BetweenFactor lc(X(5), X(1), loop, betweenNoise2); - - // Create graph - NonlinearFactorGraph graph; - graph.push_back(factor); - graph.push_back(b1); - graph.push_back(b2); - graph.push_back(b3); - graph.push_back(b4); - graph.push_back(lc); - - //graph.print("Full Graph\n"); - Values initial; - initial.insert(X(1), Similarity3()); - initial.insert(X(2), - Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1)); - initial.insert(X(3), - Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); - initial.insert(X(4), - Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3)); - initial.insert(X(5), - Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); - - //initial.print("Initial Estimate\n"); - - Values result; - result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - //result.print("Optimized Estimate\n"); - Pose3 p1, p2, p3, p4, p5; - p1 = Pose3(result.at(X(1))); - p2 = Pose3(result.at(X(2))); - p3 = Pose3(result.at(X(3))); - p4 = Pose3(result.at(X(4))); - p5 = Pose3(result.at(X(5))); - - //p1.print("Pose1"); - //p2.print("Pose2"); - //p3.print("Pose3"); - //p4.print("Pose4"); - //p5.print("Pose5"); - - Similarity3 expected(0.7); - EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); -} - -//****************************************************************************** -// Align points (p,q) assuming that p = T*q + noise -TEST(Similarity3, AlignScaledPointClouds) { -// Create ground truth - Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0); - - // Create transformed cloud (noiseless) -// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3; - - // Create an unknown expression - Expression unknownT(0); // use key 0 - - // Create constant expressions for the ground truth points - Expression q1_(q1), q2_(q2), q3_(q3); - - // Create prediction expressions - Expression predict1(unknownT, &Similarity3::transform_from, q1_); - Expression predict2(unknownT, &Similarity3::transform_from, q2_); - Expression predict3(unknownT, &Similarity3::transform_from, q3_); - -//// Create Expression factor graph -// ExpressionFactorGraph graph; -// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1| -// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2| -// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3| -} - -//****************************************************************************** -TEST(Similarity3 , Invariants) { - Similarity3 id; - - EXPECT(check_group_invariants(id, id)); - EXPECT(check_group_invariants(id, T3)); - EXPECT(check_group_invariants(T2, id)); - EXPECT(check_group_invariants(T2, T3)); - - EXPECT(check_manifold_invariants(id, id)); - EXPECT(check_manifold_invariants(id, T3)); - EXPECT(check_manifold_invariants(T2, id)); - EXPECT(check_manifold_invariants(T2, T3)); -} - -//****************************************************************************** -TEST(Similarity3 , LieGroupDerivatives) { - Similarity3 id; - - CHECK_LIE_GROUP_DERIVATIVES(id, id); - CHECK_LIE_GROUP_DERIVATIVES(id, T2); - CHECK_LIE_GROUP_DERIVATIVES(T2, id); - CHECK_LIE_GROUP_DERIVATIVES(T2, T3); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSimilarity3.cpp + * @brief Unit tests for Similarity3 class + * @author Paul Drews + * @author Zhaoyang Lv + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace gtsam; +using namespace std; +using symbol_shorthand::X; + +GTSAM_CONCEPT_TESTABLE_INST(Similarity3) + +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const double s = 4; +static const Similarity3 id; +static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), + Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); +static const Similarity3 T4(R, P, s); +static const Similarity3 T5(R, P, 10); +static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform + +//****************************************************************************** +TEST(Similarity3, Concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Similarity3, Constructors) { + Similarity3 sim3_Construct1; + Similarity3 sim3_Construct2(s); + Similarity3 sim3_Construct3(R, P, s); + Similarity3 sim4_Construct4(R.matrix(), P, s); +} + +//****************************************************************************** +TEST(Similarity3, Getters) { + Similarity3 sim3_default; + EXPECT(assert_equal(Rot3(), sim3_default.rotation())); + EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); + EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); + + Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation())); + EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); +} + +//****************************************************************************** +TEST(Similarity3, AdjointMap) { + const Matrix4 T = T2.matrix(); + // Check Ad with actual definition + Vector7 delta; + delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + Matrix4 W = Similarity3::wedge(delta); + Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); + EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); +} + +//****************************************************************************** +TEST(Similarity3, inverse) { + Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Matrix3 Re; // some values from matlab + Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120; + Vector3 te(-9.8472, 59.7640, 10.2125); + Similarity3 expected(Re, te, 1.0 / 7.0); + EXPECT(assert_equal(expected, sim3.inverse(), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8)); + + // test lie group inverse + Matrix H1, H2; + EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8)); +} + +//****************************************************************************** +TEST(Similarity3, Multiplication) { + Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11); + Matrix3 re; + re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530; + Vector3 te(-13.6797, 3.2441, -5.7794); + Similarity3 expected(re, te, 77); + EXPECT(assert_equal(expected, test1 * test2, 1e-2)); +} + +//****************************************************************************** +TEST(Similarity3, Manifold) { + EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); + Vector z = Vector7::Zero(); + Similarity3 sim; + EXPECT(sim.retract(z) == sim); + + Vector7 v = Vector7::Zero(); + v(6) = 2; + Similarity3 sim2; + EXPECT(sim2.retract(z) == sim2); + + EXPECT(assert_equal(z, sim2.localCoordinates(sim))); + + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); + Vector v3(7); + v3 << 0, 0, 0, 1, 2, 3, 0; + EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); + + Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1); + + Vector vlocal = sim.localCoordinates(other); + + EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); + + Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1); + Rot3 R = Rot3::Rodrigues(0.3, 0, 0); + + Vector vlocal2 = sim.localCoordinates(other2); + + EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); + + // TODO add unit tests for retract and localCoordinates +} + +//****************************************************************************** +TEST( Similarity3, retract_first_order) { + Similarity3 id; + Vector v = Z_7x1; + v(0) = 0.3; + EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); +// v(3) = 0.2; +// v(4) = 0.7; +// v(5) = -2; +// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); +} + +//****************************************************************************** +TEST(Similarity3, localCoordinates_first_order) { + Vector7 d12 = Vector7::Constant(0.1); + d12(6) = 1.0; + Similarity3 t1 = T1, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} + +//****************************************************************************** +TEST(Similarity3, manifold_first_order) { + Similarity3 t1 = T1; + Similarity3 t2 = T3; + Similarity3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} + +//****************************************************************************** +// Return as a 4*4 Matrix +TEST(Similarity3, Matrix) { + Matrix4 expected; + expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5; + Matrix4 actual = T6.matrix(); + EXPECT(assert_equal(expected, actual)); +} + +//***************************************************************************** +// Exponential and log maps +TEST(Similarity3, ExpLogMap) { + Vector7 delta; + delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); + EXPECT(assert_equal(delta, actual)); + + Vector7 zeros; + zeros << 0, 0, 0, 0, 0, 0, 0; + Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); + EXPECT(assert_equal(zeros, logIdentity)); + + Similarity3 expZero = Similarity3::Expmap(zeros); + Similarity3 ident = Similarity3::identity(); + EXPECT(assert_equal(expZero, ident)); + + // Compare to matrix exponential, using expm in Lie.h + EXPECT( + assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); +} + +//****************************************************************************** +// Group action on Point3 (with simpler transform) +TEST(Similarity3, GroupAction) { + EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0))); + EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0))); + + // Test group action on R^4 via matrix representation + Vector4 qh; + qh << 1, 0, 0, 1; + Vector4 ph; + ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0) + EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); + + // Test some more... + Point3 pa = Point3(1, 0, 0); + Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); + Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); + EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); + + Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0); + Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); + EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); + + // Test derivative + boost::function f = boost::bind( + &Similarity3::transform_from, _1, _2, boost::none, boost::none); + + Point3 q(1, 2, 3); + for (const auto T : { T1, T2, T3, T4, T5, T6 }) { + Point3 q(1, 0, 0); + Matrix H1 = numericalDerivative21(f, T, q); + Matrix H2 = numericalDerivative22(f, T, q); + Matrix actualH1, actualH2; + T.transform_from(q, actualH1, actualH2); + EXPECT(assert_equal(H1, actualH1)); + EXPECT(assert_equal(H2, actualH2)); + } +} + +//****************************************************************************** +// Test very simple prior optimization example +TEST(Similarity3, Optimization) { + // Create a PriorFactor with a Sim3 prior + Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + Symbol key('x', 1); + PriorFactor factor(key, prior, model); + + // Create graph + NonlinearFactorGraph graph; + graph.push_back(factor); + + // Create initial estimate with identity transform + Values initial; + initial.insert(key, Similarity3()); + + // Optimize + Values result; + LevenbergMarquardtParams params; + params.setVerbosityLM("TRYCONFIG"); + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // After optimization, result should be prior + EXPECT(assert_equal(prior, result.at(key), 1e-4)); +} + +//****************************************************************************** +// Test optimization with both Prior and BetweenFactors +TEST(Similarity3, Optimization2) { + Similarity3 prior = Similarity3(); + Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0), + 1.0); + Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(sqrt(8) * 0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0), + Point3(sqrt(32) * 0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(6 * 0.7, 0, 0), 1.0); + Similarity3 loop = Similarity3(1.42); + + //prior.print("Goal Transform"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, + 0.01); + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); + SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); + PriorFactor factor(X(1), prior, model); // Prior ! + BetweenFactor b1(X(1), X(2), m1, betweenNoise); + BetweenFactor b2(X(2), X(3), m2, betweenNoise); + BetweenFactor b3(X(3), X(4), m3, betweenNoise); + BetweenFactor b4(X(4), X(5), m4, betweenNoise); + BetweenFactor lc(X(5), X(1), loop, betweenNoise2); + + // Create graph + NonlinearFactorGraph graph; + graph.push_back(factor); + graph.push_back(b1); + graph.push_back(b2); + graph.push_back(b3); + graph.push_back(b4); + graph.push_back(lc); + + //graph.print("Full Graph\n"); + Values initial; + initial.insert(X(1), Similarity3()); + initial.insert(X(2), + Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), + Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), + Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), + Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); + + //initial.print("Initial Estimate\n"); + + Values result; + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + //result.print("Optimized Estimate\n"); + Pose3 p1, p2, p3, p4, p5; + p1 = Pose3(result.at(X(1))); + p2 = Pose3(result.at(X(2))); + p3 = Pose3(result.at(X(3))); + p4 = Pose3(result.at(X(4))); + p5 = Pose3(result.at(X(5))); + + //p1.print("Pose1"); + //p2.print("Pose2"); + //p3.print("Pose3"); + //p4.print("Pose4"); + //p5.print("Pose5"); + + Similarity3 expected(0.7); + EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); +} + +//****************************************************************************** +// Align points (p,q) assuming that p = T*q + noise +TEST(Similarity3, AlignScaledPointClouds) { +// Create ground truth + Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0); + + // Create transformed cloud (noiseless) +// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3; + + // Create an unknown expression + Expression unknownT(0); // use key 0 + + // Create constant expressions for the ground truth points + Expression q1_(q1), q2_(q2), q3_(q3); + + // Create prediction expressions + Expression predict1(unknownT, &Similarity3::transform_from, q1_); + Expression predict2(unknownT, &Similarity3::transform_from, q2_); + Expression predict3(unknownT, &Similarity3::transform_from, q3_); + +//// Create Expression factor graph +// ExpressionFactorGraph graph; +// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1| +// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2| +// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3| +} + +//****************************************************************************** +TEST(Similarity3 , Invariants) { + Similarity3 id; + + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T3)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T3)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T3)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T3)); +} + +//****************************************************************************** +TEST(Similarity3 , LieGroupDerivatives) { + Similarity3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T3); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 39c910826..f6fc99512 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -516,73 +516,73 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { // FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); // FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); // }; -// +// // class FixedLagSmootherKeyTimestampMap { // FixedLagSmootherKeyTimestampMap(); // FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); -// +// // // Note: no print function -// +// // // common STL methods // size_t size() const; // bool empty() const; // void clear(); -// +// // double at(const gtsam::Key& key) const; // void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); // }; -// +// // class FixedLagSmootherResult { // size_t getIterations() const; // size_t getNonlinearVariables() const; // size_t getLinearVariables() const; // double getError() const; // }; -// +// // #include // virtual class FixedLagSmoother { // void print(string s) const; // bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; -// +// // gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; // double smootherLag() const; -// +// // gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); // gtsam::Values calculateEstimate() const; // }; -// +// // #include // virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { // BatchFixedLagSmoother(); // BatchFixedLagSmoother(double smootherLag); // BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// +// // gtsam::LevenbergMarquardtParams params() const; // }; -// +// // #include // virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { // IncrementalFixedLagSmoother(); // IncrementalFixedLagSmoother(double smootherLag); // IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// +// // gtsam::ISAM2Params params() const; // }; -// +// // #include // virtual class ConcurrentFilter { // void print(string s) const; // bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; // }; -// +// // virtual class ConcurrentSmoother { // void print(string s) const; // bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; // }; -// +// // // Synchronize function // void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// +// // #include // class ConcurrentBatchFilterResult { // size_t getIterations() const; @@ -591,23 +591,23 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { // size_t getLinearVariables() const; // double getError() const; // }; -// +// // virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { // ConcurrentBatchFilter(); // ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// +// // gtsam::NonlinearFactorGraph getFactors() const; // gtsam::Values getLinearizationPoint() const; // gtsam::Ordering getOrdering() const; // gtsam::VectorValues getDelta() const; -// +// // gtsam::ConcurrentBatchFilterResult update(); // gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); // gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); // gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); // gtsam::Values calculateEstimate() const; // }; -// +// // #include // class ConcurrentBatchSmootherResult { // size_t getIterations() const; @@ -616,16 +616,16 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { // size_t getLinearVariables() const; // double getError() const; // }; -// +// // virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { // ConcurrentBatchSmoother(); // ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// +// // gtsam::NonlinearFactorGraph getFactors() const; // gtsam::Values getLinearizationPoint() const; // gtsam::Ordering getOrdering() const; // gtsam::VectorValues getDelta() const; -// +// // gtsam::ConcurrentBatchSmootherResult update(); // gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); // gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index a5c60f311..8c3c5a7e5 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -28,7 +28,7 @@ namespace gtsam { * This class implements the active set algorithm for solving convex * Programming problems. * - * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or + * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or * QP (quadratic program). * @tparam POLICY specific detail policy tailored for the particular program * @tparam INITSOLVER Solver for an initial feasible solution of this problem. @@ -70,7 +70,7 @@ protected: dual graphs */ /// Vector of key matrix pairs. Matrices are usually the A term for a factor. - typedef std::vector > TermsContainer; + typedef std::vector > TermsContainer; public: /// Constructor @@ -100,18 +100,18 @@ public: protected: /** - * Compute minimum step size alpha to move from the current point @p xk to the - * next feasible point along a direction @p p: x' = xk + alpha*p, - * where alpha \in [0,maxAlpha]. - * + * Compute minimum step size alpha to move from the current point @p xk to the + * next feasible point along a direction @p p: x' = xk + alpha*p, + * where alpha \in [0,maxAlpha]. + * * For QP, maxAlpha = 1. For LP: maxAlpha = Inf. * * @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex - * is the closest inactive inequality constraint that blocks xk to move - * further and that has the minimum alpha, or (-1, maxAlpha) if there is no + * is the closest inactive inequality constraint that blocks xk to move + * further and that has the minimum alpha, or (-1, maxAlpha) if there is no * such inactive blocking constraint. - * - * 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. */ boost::tuple computeStepSize( @@ -119,15 +119,15 @@ protected: const VectorValues& p, const double& maxAlpha) const; /** - * Finds the active constraints in the given factor graph and returns the + * Finds the active constraints in the given factor graph and returns the * Dual Jacobians used to build a dual factor graph. */ template TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, const VariableIndex& variableIndex) const { /* - * Iterates through each factor in the factor graph and checks - * whether it's active. If the factor is active it reutrns the A + * Iterates through each factor in the factor graph and checks + * whether it's active. If the factor is active it reutrns the A * term of the factor. */ TermsContainer Aterms; @@ -167,7 +167,7 @@ public: /// Just for testing... GaussianFactorGraph buildWorkingGraph( const InequalityFactorGraph& workingSet, const VectorValues& xk = VectorValues()) const; - + /// Iterate 1 step, return a new state with a new workingSet and values State iterate(const State& state) const; diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h index f7c5a5c79..2184e9f52 100644 --- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -16,7 +16,7 @@ * @date 1/24/16 */ -#pragma once +#pragma once namespace gtsam { diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index c1319a5ec..59b55cebd 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -11,7 +11,7 @@ /** * @file LPSolver.cpp - * @brief + * @brief * @author Duy Nguyen Ta * @author Ivan Dario Jimenez * @date 1/26/16 diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 153aa7fda..460b4b7ee 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -58,7 +58,7 @@ struct LPPolicy { allKeys.merge(lp.equalities.keys()); allKeys.merge(KeySet(lp.cost.keys())); // Add corresponding factors for all variables that are not explicitly in - // the cost function. Gradients of the cost function wrt to these variables + // the cost function. Gradients of the cost function wrt to these variables // are zero (g=0), so b=xk if (lp.cost.keys().size() != allKeys.size()) { KeySet difference; diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp index 47672a947..0d6982bfd 100644 --- a/gtsam_unstable/linear/RawQP.cpp +++ b/gtsam_unstable/linear/RawQP.cpp @@ -11,7 +11,7 @@ /** * @file RawQP.cpp - * @brief + * @brief * @author Ivan Dario Jimenez * @date 3/5/16 */ diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h index 70b2a9d27..6d36f57ad 100644 --- a/gtsam_unstable/linear/RawQP.h +++ b/gtsam_unstable/linear/RawQP.h @@ -11,7 +11,7 @@ /** * @file RawQP.h - * @brief + * @brief * @author Ivan Dario Jimenez * @date 3/5/16 */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 51a959075..215e63f0e 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 53dd46fea..d04f579eb 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 0fc975958..775dccbb0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -270,7 +270,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { while(true) { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "trying lambda = " << lambda << std::endl; - + // Add prior factors at the current solution gttic(damp); GaussianFactorGraph dampedFactorGraph(linearFactorGraph); @@ -287,7 +287,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { } } gttoc(damp); - if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) + if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) dampedFactorGraph.print("damped"); result.lambdas++; @@ -298,9 +298,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { evalpoint = theta_.retract(newDelta); gttoc(solve); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "linear delta norm = " << newDelta.norm() << std::endl; - if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) + if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) newDelta.print("delta"); // Evaluate the new error @@ -308,9 +308,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "next error = " << error << std::endl; - + if(error < result.error) { // Keep this change // Update the error value diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index f3808686c..67a1ef4f3 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 70cb3e268..66d03bfa2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index b8a9941ad..42f82f52f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index e7c8229ef..78bd977f5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index c28b3bcd1..e01919048 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 41a94b876..714d7c8d0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 2d519bf25..ec95e1ec8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index e285955a7..1a86adbfa 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index f9fa6033f..ece8cd2f6 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 25dd5fe98..cd36da398 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 54ad22bcb..ff5a096b0 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 8ecd087c5..47002acb6 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index d6f6446e8..342e2e79f 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index d7bddece2..98b4bf8cc 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 82d8f2153..d4ebc7c0a 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index 440400fb4..937108a67 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 0b86a60e9..65df6261c 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 0e22edf0f..2088ee646 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -60,7 +60,7 @@ namespace gtsam { namespace partition { } // call metis parition routine - METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), + METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), vwgt, options, &sepsize, part_.get()); if (verbose) { @@ -95,7 +95,7 @@ namespace gtsam { namespace partition { ncon = graph->ncon; ctrl->ncuts = 1; - + /* determine the weights of the two partitions as a function of the weight of the target partition weights */ @@ -153,7 +153,7 @@ namespace gtsam { namespace partition { modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), options, &edgecut, part_.get()); - + if (verbose) { //stoptimer(TOTALTmr); printf("\nTiming Information --------------------------------------------------\n"); @@ -198,7 +198,7 @@ namespace gtsam { namespace partition { std::cout << "index1: " << index1 << std::endl; std::cout << "index2: " << index2 << std::endl; // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator - if (index1 >= 0 && index2 >= 0) { + if (index1 >= 0 && index2 >= 0) { std::pair& adjacencyMap1 = adjacencyMap[index1]; std::pair& adjacencyMap2 = adjacencyMap[index2]; try{ @@ -239,7 +239,7 @@ namespace gtsam { namespace partition { const std::vector& keys, WorkSpace& workspace, bool verbose) { // create a metis graph size_t numKeys = keys.size(); - if (verbose) + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; sharedInts xadj, adjncy, adjwgt; @@ -287,7 +287,7 @@ namespace gtsam { namespace partition { return result; } - /* *************************************************************************/ + /* *************************************************************************/ template boost::optional edgePartitionByMetis(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose) { @@ -492,7 +492,7 @@ namespace gtsam { namespace partition { const boost::optional >& int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - boost::optional result = findPartitoning(graph, keys, workspace, + boost::optional result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph); // find the island in A and B, and make them separated submaps diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 9706e4cc4..fe49de928 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -121,7 +121,7 @@ TEST ( Partition, edgePartitionByMetis2 ) CHECK(A_expected == actual->A); CHECK(B_expected == actual->B); CHECK(C_expected == actual->C); -} +} /* ************************************************************************* */ // x0 - x1 - x2 diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index b009927d6..104b3209d 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index df1873765..f2bcb7cd7 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index 0b0a1bd6c..9f749e9e5 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -2,7 +2,7 @@ * @file DummyFactor.h * * @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected. - * + * * @date Sep 10, 2012 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index d2a9110d9..03803b5f4 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 2de060302..d8649a0d5 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index 407652583..3d81fbab3 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -2,7 +2,7 @@ * @file RelativeElevationFactor.h * * @brief Factor representing a known relative altitude in global frame - * + * * @date Aug 17, 2012 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index a844ee823..b3d71d05f 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -2,7 +2,7 @@ * @file SmartRangeFactor.h * * @brief A smart factor for range-only SLAM that does initialization and marginalization - * + * * @date Sep 10, 2012 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index f32dd3b3e..2ceba2a97 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -360,7 +360,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - FBlocks& Fs, + FBlocks& Fs, Matrix& E, Vector& b, const Cameras& cameras) const { diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index e37e8ff20..1df14a8de 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 6c2f55195..d59e33724 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 156ac242e..c29e3e794 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index b6d906fd4..48aaa8ed7 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/serialization.h b/gtsam_unstable/slam/serialization.h index 08451fa0c..1752aac80 100644 --- a/gtsam_unstable/slam/serialization.h +++ b/gtsam_unstable/slam/serialization.h @@ -2,7 +2,7 @@ * @file serialization.h * * @brief Global functions for performing serialization, designed for use with matlab - * + * * @date Jun 12, 2013 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index c85187d5f..e4112f53d 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -1,6 +1,6 @@ /** * @file testBiasedGPSFactor.cpp - * @brief + * @brief * @author Luca Carlone * @date July 30, 2014 */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 99f494ad9..a74bfc3c6 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -56,7 +56,7 @@ TEST( InvDepthFactor, optimize) { LevenbergMarquardtParams lmParams; Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - + // with a single factor the incorrect initialization of 1/4 should not move! EXPECT(assert_equal(initial, result, 1e-9)); @@ -80,12 +80,12 @@ TEST( InvDepthFactor, optimize) { initial.insert(Symbol('x',2), right_pose); Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - + Point3 result2_lmk = InvDepthCamera3::invDepthTo3D( result2.at(Symbol('l',1)), result2.at(Symbol('d',1))); EXPECT(assert_equal(landmark, result2_lmk, 1e-9)); - + // TODO: need to add priors to make this work with // Values result2 = optimize(graph, initial, // NonlinearOptimizationParameters(),MULTIFRONTAL, GN); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index a6eed54d5..976a3f43d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -46,7 +46,7 @@ TEST( InvDepthFactorVariant3, optimize) { Vector3 expected((Vector(3) << theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -72,7 +72,7 @@ TEST( InvDepthFactorVariant3, optimize) { LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Vector3 actual = result.at(landmarkKey); - + // Test that the correct landmark parameters have been recovered diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index b468a2b6e..f589e932f 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index ddb5cf7a2..166f058e3 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 573352e87..4e9fdcb50 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index e878ea5d9..5eadf5fd6 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index e18505af6..ead807138 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 233dbdceb..4b10d5c0b 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index ebb52d276..098a0ef56 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index f39bdda59..d05869a9c 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp index 1c3582813..379488631 100644 --- a/python/handwritten/base/FastVector.cpp +++ b/python/handwritten/base/FastVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/common.h b/python/handwritten/common.h index 72d2ae846..2690d52fc 100644 --- a/python/handwritten/common.h +++ b/python/handwritten/common.h @@ -17,7 +17,7 @@ #pragma once /* Fix to avoid registration warnings */ -// Solution taken from https://github.com/BVLC/caffe/pull/4069/commits/673e8cfc0b8f05f9fa3ebbad7cc6202822e5d9c5 +// Solution taken from https://github.com/BVLC/caffe/pull/4069/commits/673e8cfc0b8f05f9fa3ebbad7cc6202822e5d9c5 #define REGISTER_SHARED_PTR_TO_PYTHON(PTR) do { \ const boost::python::type_info info = \ boost::python::type_id >(); \ diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp index 440b21742..1d59b1780 100644 --- a/python/handwritten/geometry/Cal3_S2.cpp +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp index 21ffcdeb7..97f6e09c6 100644 --- a/python/handwritten/geometry/PinholeCamera.cpp +++ b/python/handwritten/geometry/PinholeCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp index 99a97adc9..e7590976f 100644 --- a/python/handwritten/geometry/Point2.cpp +++ b/python/handwritten/geometry/Point2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp index 4f402df7e..5b4b4731b 100644 --- a/python/handwritten/geometry/Pose2.cpp +++ b/python/handwritten/geometry/Pose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transfor BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) -// Manually wrap +// Manually wrap void exportPose2(){ @@ -61,9 +61,9 @@ void exportPose2(){ // .def("dim", &Pose2::dim) // .def("retract", &Pose2::retract) - .def("transform_to", &Pose2::transform_to, + .def("transform_to", &Pose2::transform_to, transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_from", &Pose2::transform_from, + .def("transform_from", &Pose2::transform_from, transform_to_overloads(args("point", "H1", "H2"))) .def("x", &Pose2::x) diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp index 59b4ce4e8..961ad970f 100644 --- a/python/handwritten/geometry/Rot2.cpp +++ b/python/handwritten/geometry/Rot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp index 9fc5b74e7..0ec3445b8 100644 --- a/python/handwritten/inference/Symbol.cpp +++ b/python/handwritten/inference/Symbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp index e918e509d..c4c46fe7f 100644 --- a/python/handwritten/linear/NoiseModel.cpp +++ b/python/handwritten/linear/NoiseModel.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -35,7 +35,7 @@ using namespace gtsam; using namespace gtsam::noiseModel; // Wrap around pure virtual class Base. -// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the // overloading through inheritance in Python. // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions struct BaseCallback : Base, wrapper @@ -76,7 +76,7 @@ struct BaseCallback : Base, wrapper }; -// Overloads for named constructors. Named constructors are static, so we declare them +// Overloads for named constructors. Named constructors are static, so we declare them // using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) @@ -97,13 +97,13 @@ void exportNoiseModels(){ object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); scope().attr("noiseModel") = noiseModel_module; scope noiseModel_scope = noiseModel_module; - + // Then export our classes in the noiseModel scope class_("Base") .def("print", pure_virtual(&Base::print)) ; register_ptr_to_python< boost::shared_ptr >(); - + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) class_, bases >("Gaussian", no_init) .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) @@ -114,7 +114,7 @@ void exportNoiseModels(){ .staticmethod("Covariance") ; REGISTER_SHARED_PTR_TO_PYTHON(Gaussian); - + class_, bases >("Diagonal", no_init) .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) .staticmethod("Sigmas") @@ -124,7 +124,7 @@ void exportNoiseModels(){ .staticmethod("Precisions") ; REGISTER_SHARED_PTR_TO_PYTHON(Diagonal); - + class_, bases >("Isotropic", no_init) .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) .staticmethod("Sigma") @@ -134,7 +134,7 @@ void exportNoiseModels(){ .staticmethod("Precision") ; REGISTER_SHARED_PTR_TO_PYTHON(Isotropic); - + class_, bases >("Unit", no_init) .def("Create",&Unit::Create) .staticmethod("Create") diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp index f80c5df99..bd908ce8f 100644 --- a/python/handwritten/nonlinear/ISAM2.cpp +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -45,7 +45,7 @@ class_("ISAM2Params") // OptimizationParams getOptimizationParams () const // void setKeyFormatter (KeyFormatter keyFormatter) // KeyFormatter getKeyFormatter () const - // GaussianFactorGraph::Eliminate getEliminationFunction () const + // GaussianFactorGraph::Eliminate getEliminationFunction () const ; // TODO(Ellon): Export useful methods/properties of ISAM2Result diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp index 8ccc123fc..359ac48f3 100644 --- a/python/handwritten/nonlinear/NonlinearFactor.cpp +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -25,7 +25,7 @@ using namespace boost::python; using namespace gtsam; // Wrap around pure virtual class NonlinearFactor. -// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the // overloading through inheritance in Python. // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions struct NonlinearFactorWrap : NonlinearFactor, wrapper diff --git a/python/handwritten/slam/GenericProjectionFactor.cpp b/python/handwritten/slam/GenericProjectionFactor.cpp index dd932ccd4..1aa66b8f3 100644 --- a/python/handwritten/slam/GenericProjectionFactor.cpp +++ b/python/handwritten/slam/GenericProjectionFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp index d7cebe7ad..3d3a15727 100644 --- a/python/handwritten/utils/NumpyEigen.cpp +++ b/python/handwritten/utils/NumpyEigen.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -40,7 +40,7 @@ void registerNumpyEigenConversions() NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); - + NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 3245652be..c4930a55b 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index 7d399dc02..deb9292f7 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 9e604cb3c..948a87ce5 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/smallExample.h b/tests/smallExample.h index d3a69b0bd..20ebcd1ba 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index a5d1a195c..c67914c8b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 984728ebf..b8b6cf284 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index eec62ff9c..0d345a726 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 07323e0fc..309f0e1e0 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index b95f16e76..ad7cabb99 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index e0c2b7b66..7afb4e178 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 2d8a0b975..0ef12198b 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 195914176..d0b4a1ffa 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index f44496e27..4bdbd9387 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testMarginals.cpp - * @brief + * @brief * @author Richard Roberts * @date May 14, 2012 */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 6e49fa749..e1156ceba 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testNonlinearFactor.cpp - * @brief Unit tests for Non-Linear Factor, + * @brief Unit tests for Non-Linear Factor, * create a non linear factor graph and a values structure for it and * calculate the error for the factor. * @author Christian Potthast diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 3d30d6880..0e7793552 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ -/** +/** * @file testNonlinearOptimizer.cpp * @brief Unit tests for NonlinearOptimizer class * @author Frank Dellaert diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 6bc155214..64e111e94 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSimulated2D.cpp b/tests/testSimulated2D.cpp index 25a91c5ce..9c342ad88 100644 --- a/tests/testSimulated2D.cpp +++ b/tests/testSimulated2D.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index d411e7d5e..837fd1689 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index dcfcc735b..02f6ed762 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index accf9a65e..ed16c7c07 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index 8950c636b..d5431cee7 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp index 208c991fd..0909ac7a5 100644 --- a/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeCholesky.cpp b/timing/timeCholesky.cpp index 4dd349593..15be64313 100644 --- a/timing/timeCholesky.cpp +++ b/timing/timeCholesky.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp index 97c40e6f4..d9c19703c 100644 --- a/timing/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp index e6ae51aa4..3aecac3c1 100644 --- a/timing/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -54,7 +54,7 @@ int main() { // create a linear factor Matrix Ax2 = (Matrix(8, 2) << - // x2 + // x2 -5., 0., +0.,-5., 10., 0., @@ -64,9 +64,9 @@ int main() 10., 0., +0.,10. ).finished(); - + Matrix Al1 = (Matrix(8, 10) << - // l1 + // l1 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., @@ -76,7 +76,7 @@ int main() 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. ).finished(); - + Matrix Ax1 = (Matrix(8, 2) << // x1 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., @@ -99,7 +99,7 @@ int main() b2(5) = 1.5; b2(6) = 2; b2(7) = -1; - + // time eliminate JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1)); long timeLog = clock(); diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index cd11f6360..1efdb9542 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index e09b83232..a5740a1f1 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -155,7 +155,7 @@ int main(int argc, char *argv[]) { Pose pose = isam2.calculateEstimate(poseKey); Rot2 measuredBearing = measurement->measured().bearing(); double measuredRange = measurement->measured().range(); - newVariables.insert(lmKey, + newVariables.insert(lmKey, pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } } diff --git a/timing/timeLinearize.h b/timing/timeLinearize.h index dfb63ffa3..a2a77168c 100644 --- a/timing/timeLinearize.h +++ b/timing/timeLinearize.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index a094f9628..c3e9f32f4 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index a2010891b..f16be40f6 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp index 962e7ee21..53510cfa5 100644 --- a/timing/timeOneCameraExpression.cpp +++ b/timing/timeOneCameraExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index badc03cf8..6af3e0163 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 8b48d4b16..387d458ea 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index 5806149f3..00c02c250 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeSFMExpressions.cpp b/timing/timeSFMExpressions.cpp index cfb8e0dc6..65241b63b 100644 --- a/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 913c11284..f85aed72e 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Argument.h b/wrap/Argument.h index 0a4ebba9d..c08eb0be9 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -101,7 +101,7 @@ struct ArgumentList: public std::vector { ArgumentList expandTemplate(const TemplateSubstitution& ts) const; bool isSameSignature(const ArgumentList& other) const { - for(size_t i = 0; i #include -#include -#include -#include +#include +#include +#include #include #include // std::ostream_iterator -//#include // on Linux GCC: fails with error regarding needing C++0x std flags -//#include // same failure as above -#include // works on Linux GCC +//#include // on Linux GCC: fails with error regarding needing C++0x std flags +//#include // same failure as above +#include // works on Linux GCC using namespace std; using namespace wrap; @@ -87,20 +87,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { - // Create namespace folders + // Create namespace folders createNamespaceStructure(namespaces(), toolboxPath); - // open destination classFile + // open destination classFile string classFile = matlabName(toolboxPath); FileWriter proxyFile(classFile, verbose_, "%"); - // get the name of actual matlab object + // get the name of actual matlab object const string matlabQualName = qualifiedName("."); const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - // emit class proxy code - // we want our class to inherit the handle class for memory purposes + // emit class proxy code + // we want our class to inherit the handle class for memory purposes const string parent = parentClass ? parentClass->qualifiedName(".") : "handle"; comment_fragment(proxyFile); @@ -110,20 +110,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - // Constructor + // Constructor proxyFile.oss << " function obj = " << name() << "(varargin)\n"; - // Special pointer constructors - one in MATLAB to create an object and - // assign a pointer returned from a C++ function. In turn this MATLAB - // constructor calls a special C++ function that just adds the object to - // its collector. This allows wrapped functions to return objects in - // other wrap modules - to add these to their collectors the pointer is - // passed from one C++ module into matlab then back into the other C++ - // module. + // Special pointer constructors - one in MATLAB to create an object and + // assign a pointer returned from a C++ function. In turn this MATLAB + // constructor calls a special C++ function that just adds the object to + // its collector. This allows wrapped functions to return objects in + // other wrap modules - to add these to their collectors the pointer is + // passed from one C++ module into matlab then back into the other C++ + // module. pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); wrapperFile.oss << "\n"; - // Regular constructors + // Regular constructors boost::optional cppBaseName = qualifiedParent(); for (size_t i = 0; i < constructor.nrOverloads(); i++) { ArgumentList args = constructor.argumentList(i); @@ -145,7 +145,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; proxyFile.oss << " end\n\n"; - // Deconstructor + // Deconstructor { const int id = (int) functionNames.size(); deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); @@ -160,7 +160,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - // Methods + // Methods for(const Methods::value_type& name_m: methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, @@ -175,7 +175,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << "\n"; proxyFile.oss << " methods(Static = true)\n"; - // Static methods + // Static methods for(const StaticMethods::value_type& name_m: static_methods) { const StaticMethod& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, @@ -190,7 +190,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - // Close file + // Close file proxyFile.emit(true); } @@ -217,8 +217,8 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, functionNames.push_back(upcastFromVoidFunctionName); } - // MATLAB constructor that assigns pointer to matlab object then calls c++ - // function to add the object to the collector. + // MATLAB constructor that assigns pointer to matlab object then calls c++ + // function to add the object to the collector. if (isVirtual) { proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; @@ -241,25 +241,25 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, proxyFile.oss << " "; else proxyFile.oss << " base_ptr = "; - proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr + proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - // C++ function to add pointer from MATLAB to collector. The pointer always - // comes from a C++ return value; this mechanism allows the object to be added - // to a collector in a different wrap module. If this class has a base class, - // a new pointer to the base class is allocated and returned. + // C++ function to add pointer from MATLAB to collector. The pointer always + // comes from a C++ return value; this mechanism allows the object to be added + // to a collector in a different wrap module. If this class has a base class, + // a new pointer to the base class is allocated and returned. wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; - // Typedef boost::shared_ptr + // Typedef boost::shared_ptr wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; wrapperFile.oss << "\n"; - // Get self pointer passed in + // Get self pointer passed in wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; - // Add to collector + // Add to collector wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; - // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) + // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) boost::optional cppBaseName = qualifiedParent(); if (cppBaseName) { wrapperFile.oss << "\n"; @@ -272,10 +272,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } wrapperFile.oss << "}\n"; - // If this is a virtual function, C++ function to dynamic upcast it from a - // shared_ptr. This mechanism allows automatic dynamic creation of the - // real underlying derived-most class when a C++ method returns a virtual - // base class. + // If this is a virtual function, C++ function to dynamic upcast it from a + // shared_ptr. This mechanism allows automatic dynamic creation of the + // real underlying derived-most class when a C++ method returns a virtual + // base class. if (isVirtual) wrapperFile.oss << "\n" "void " << upcastFromVoidFunctionName diff --git a/wrap/Class.h b/wrap/Class.h index 910ecde57..3df37fe67 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -72,9 +72,9 @@ public: // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] - std::vector templateInstTypeList; ///< the original typelist used to instantiate this class from a template. + std::vector templateInstTypeList; ///< the original typelist used to instantiate this class from a template. ///< Empty if it's not an instantiation. Needed for template classes in Cython pxd. - boost::optional templateClass = boost::none; ///< qualified name of the original template class from which this class was instantiated. + boost::optional templateClass = boost::none; ///< qualified name of the original template class from which this class was instantiated. ///< boost::none if not an instantiation. Needed for template classes in Cython pxd. bool isVirtual; ///< Whether the class is part of a virtual inheritance chain bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 4292d5645..172cd24a4 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp index 9e6495602..7bb366e3f 100644 --- a/wrap/Deconstructor.cpp +++ b/wrap/Deconstructor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -48,11 +48,11 @@ string Deconstructor::wrapper_fragment(FileWriter& file, const string& cppClassName, const string& matlabUniqueName, int id) const { - + const string matlabName = matlab_wrapper_name(matlabUniqueName); const string wrapFunctionName = matlabUniqueName + "_deconstructor_" + boost::lexical_cast(id); - + file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; file.oss << "{" << endl; file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; diff --git a/wrap/Deconstructor.h b/wrap/Deconstructor.h index f0dbc2709..ee2f4ea19 100644 --- a/wrap/Deconstructor.h +++ b/wrap/Deconstructor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 127823e82..190387ecc 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index 87c5169dd..d6214ef2e 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 8da667fae..80b0adbbe 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Function.h b/wrap/Function.h index ad57a28c8..c39b3231c 100644 --- a/wrap/Function.h +++ b/wrap/Function.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index c8482f2c4..6da729607 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -136,7 +136,7 @@ void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const { /* ************************************************************************* */ void GlobalFunction::emit_cython_pxd(FileWriter& file) const { file.oss << "cdef extern from \"" << includeFile << "\" namespace \"" - << overloads[0].qualifiedNamespaces("::") + << overloads[0].qualifiedNamespaces("::") << "\":" << endl; for (size_t i = 0; i < nrOverloads(); ++i) { file.oss << " "; diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 473ebadef..9742733f7 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -2,7 +2,7 @@ * @file GlobalFunction.h * * @brief Implements codegen for a global function wrapped in matlab - * + * * @date Jul 22, 2012 * @author Alex Cunningham */ diff --git a/wrap/Method.h b/wrap/Method.h index bfa4a65da..88a73cd80 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Module.h b/wrap/Module.h index 732b66507..2a8344551 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -84,7 +84,7 @@ private: const std::vector instantiations); static std::vector GenerateValidTypes( const std::vector& classes, - const std::vector& forwardDeclarations, + const std::vector& forwardDeclarations, const std::vector& typedefs); static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes); diff --git a/wrap/Qualified.h b/wrap/Qualified.h index 4dc15dda1..416db239d 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -95,7 +95,7 @@ public: } bool match(const std::vector& templateArgs) const { - for(const std::string& s: templateArgs) + for(const std::string& s: templateArgs) if (match(s)) return true; return false; } diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index 506e7d471..c8f8c1cca 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -91,7 +91,7 @@ std::string ReturnType::pyx_returnType(bool addShared) const { /* ************************************************************************* */ std::string ReturnType::pyx_casting(const std::string& var, bool isSharedVar) const { - if (isEigen()) { + if (isEigen()) { string s = "ndarray_copy(" + var + ")"; if (pyxClassName() == "Vector") return s + ".squeeze()"; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 4fe273dee..0f812ea61 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index cbcfc8d49..8392a1cc5 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Template.h b/wrap/Template.h index 991c6c883..32f8e9761 100644 --- a/wrap/Template.h +++ b/wrap/Template.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index 6aa35a5bd..3f9b5de10 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateInstantiationTypedef.h b/wrap/TemplateInstantiationTypedef.h index 2a08d7a94..dfec272e3 100644 --- a/wrap/TemplateInstantiationTypedef.h +++ b/wrap/TemplateInstantiationTypedef.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateMethod.cpp b/wrap/TemplateMethod.cpp index d683fed50..10d2371c8 100644 --- a/wrap/TemplateMethod.cpp +++ b/wrap/TemplateMethod.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -16,7 +16,7 @@ #include "TemplateMethod.h" #include "Class.h" - + using namespace std; using namespace wrap; diff --git a/wrap/TemplateMethod.h b/wrap/TemplateMethod.h index 896074baa..399eeec5f 100644 --- a/wrap/TemplateMethod.h +++ b/wrap/TemplateMethod.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateSubstitution.h b/wrap/TemplateSubstitution.h index 7fe00e213..f214bc607 100644 --- a/wrap/TemplateSubstitution.h +++ b/wrap/TemplateSubstitution.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp index ee98480c1..539f4b8ec 100644 --- a/wrap/TypeAttributesTable.cpp +++ b/wrap/TypeAttributesTable.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TypeAttributesTable.h b/wrap/TypeAttributesTable.h index 132e2cda1..4bbc239b4 100644 --- a/wrap/TypeAttributesTable.h +++ b/wrap/TypeAttributesTable.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/matlab.h b/wrap/matlab.h index de3027eac..1af00bd00 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -110,7 +110,7 @@ protected: //***************************************************************************** void checkArguments(const string& name, int nargout, int nargin, int expected) { - stringstream err; + stringstream err; err << name << " expects " << expected << " arguments, not " << nargin; if (nargin!=expected) error(err.str().c_str()); diff --git a/wrap/spirit.h b/wrap/spirit.h index 98113a1f4..202b7e56d 100644 --- a/wrap/spirit.h +++ b/wrap/spirit.h @@ -16,7 +16,7 @@ #include #include #include - + namespace boost { namespace spirit { diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index ba06d1309..54691ffde 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -120,7 +120,7 @@ void _geometry_RTTIRegister() { if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); mxDestroyArray(registry); - + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); diff --git a/wrap/tests/testArgument.cpp b/wrap/tests/testArgument.cpp index d99f9e4b3..555401309 100644 --- a/wrap/tests/testArgument.cpp +++ b/wrap/tests/testArgument.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 3d8d79b33..dbf2e8e7a 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -207,7 +207,7 @@ TEST( Class, TemplateSubstition ) { // check the number of new classes is four EXPECT_LONGS_EQUAL(4, classes.size()); - // check return types + // check return types EXPECT( classes[0].method("myMethod").returnValue(0).type1 == q_void); EXPECT( classes[1].method("myMethod").returnValue(0).type1 == q_double); EXPECT( classes[2].method("myMethod").returnValue(0).type1 == q_Matrix); diff --git a/wrap/tests/testGlobalFunction.cpp b/wrap/tests/testGlobalFunction.cpp index e707b6ee7..c336afd32 100644 --- a/wrap/tests/testGlobalFunction.cpp +++ b/wrap/tests/testGlobalFunction.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testMethod.cpp b/wrap/tests/testMethod.cpp index d823e9099..f45869728 100644 --- a/wrap/tests/testMethod.cpp +++ b/wrap/tests/testMethod.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testReturnValue.cpp b/wrap/tests/testReturnValue.cpp index 720b120b4..62a8ff0c7 100644 --- a/wrap/tests/testReturnValue.cpp +++ b/wrap/tests/testReturnValue.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 27c9be6d6..b9e01a17e 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -68,7 +68,7 @@ TEST( spirit, sequence ) { // parser for interface files // const string reference reference -Rule constStringRef_p = +Rule constStringRef_p = str_p("const") >> "string" >> '&'; // class reference @@ -104,9 +104,9 @@ TEST( spirit, constMethod_p ) { /* ************************************************************************* */ -/* See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56665 +/* See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56665 GCC compiler issues with -O2 and -fno-strict-aliasing results in undefined - behaviour when spirit uses assign_a with a literal. + behaviour when spirit uses assign_a with a literal. GCC versions 4.7.2 -> 5.4 inclusive */ TEST( spirit, return_value_p ) { diff --git a/wrap/tests/testTemplate.cpp b/wrap/tests/testTemplate.cpp index eed144677..1c735f3f2 100644 --- a/wrap/tests/testTemplate.cpp +++ b/wrap/tests/testTemplate.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testType.cpp b/wrap/tests/testType.cpp index f06a88962..229b79728 100644 --- a/wrap/tests/testType.cpp +++ b/wrap/tests/testType.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8668ec88f..64f28db83 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 32ee85eee..1127fa77b 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -110,7 +110,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) /* ************************************************************************* */ string maybe_shared_ptr(bool add, const string& qtype, const string& type) { string str = add? "Shared" : ""; - if (add) str += type; + if (add) str += type; else str += qtype; return str; } diff --git a/wrap/utilities.h b/wrap/utilities.h index d8108d6a5..17c5cba12 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -90,7 +90,7 @@ public: ~AttributeError() throw() {} virtual const char* what() const throw() { return what_.c_str(); } }; - + // "Unique" key to signal calling the matlab object constructor with a raw pointer // to a shared pointer of the same C++ object type as the MATLAB type. // Also present in matlab.h diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index fe17e1c66..5874f52d0 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list)