Merged from branch 'trunk'
Richard Roberts
commit
123657e3d0
116
.cproject
116
.cproject
|
|
@ -1,5 +1,7 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
|
||||
|
|
@ -451,14 +453,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testVelocityConstraint3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -619,6 +613,54 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testPlanarSLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testPose2SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testPose3SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testVisualSLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -627,18 +669,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
|
||||
<buildTarget>testSerializationSLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSerialization.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSerialization.run</buildTarget>
|
||||
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -795,6 +837,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testImuFactor.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j8</buildArguments>
|
||||
<buildTarget>testImuFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -875,14 +925,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBayesTreeOperations.run" path="build/gtsam_unstable/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testBayesTreeOperations.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1115,14 +1157,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSerializationSLAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSerializationSLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1587,14 +1621,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3Q.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testRot3Q.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1912,6 +1938,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="RangeISAMExample_plaza2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>RangeISAMExample_plaza2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testImuFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@ protected:
|
|||
/**
|
||||
* Normal test will wrap execution in a try/catch block to catch exceptions more effectively
|
||||
*/
|
||||
#define TEST(testName, testGroup)\
|
||||
#define TEST(testGroup, testName)\
|
||||
class testGroup##testName##Test : public Test \
|
||||
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
|
||||
virtual ~testGroup##testName##Test () {};\
|
||||
|
|
@ -72,7 +72,7 @@ protected:
|
|||
* For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this
|
||||
* will not wrap execution with a try/catch block
|
||||
*/
|
||||
#define TEST_UNSAFE(testName, testGroup)\
|
||||
#define TEST_UNSAFE(testGroup, testName)\
|
||||
class testGroup##testName##Test : public Test \
|
||||
{ public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
|
||||
virtual ~testGroup##testName##Test () {};\
|
||||
|
|
|
|||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,207 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 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 RangeISAMExample_plaza1.cpp
|
||||
* @brief A 2D Range SLAM example
|
||||
* @date June 20, 2013
|
||||
* @author FRank Dellaert
|
||||
*/
|
||||
|
||||
// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
// Each variable in the system (poses and landmarks) must be identified with a unique key.
|
||||
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
// Here we will use Symbols
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
||||
// We want to use iSAM2 to solve the range-SLAM problem incrementally
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
|
||||
// and initial guesses for any new variables used in the added factors
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
// We will use a non-liear solver to batch-inituialize from the first 150 frames
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics SLAM problems.
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/RangeFactor.h>
|
||||
|
||||
// Standard headers, added last, so we know headers above work on their own
|
||||
#include <boost/foreach.hpp>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
namespace NM = gtsam::noiseModel;
|
||||
|
||||
// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
|
||||
// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
|
||||
|
||||
// load the odometry
|
||||
// DR: Odometry Input (delta distance traveled and delta heading change)
|
||||
// Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
|
||||
typedef pair<double, Pose2> TimedOdometry;
|
||||
list<TimedOdometry> readOdometry() {
|
||||
list<TimedOdometry> odometryList;
|
||||
ifstream is("../../examples/Data/Plaza2_DR.txt");
|
||||
if (!is)
|
||||
throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
|
||||
|
||||
while (is) {
|
||||
double t, distance_traveled, delta_heading;
|
||||
is >> t >> distance_traveled >> delta_heading;
|
||||
odometryList.push_back(
|
||||
TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
return odometryList;
|
||||
}
|
||||
|
||||
// load the ranges from TD
|
||||
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||
typedef boost::tuple<double, size_t, double> RangeTriple;
|
||||
vector<RangeTriple> readTriples() {
|
||||
vector<RangeTriple> triples;
|
||||
ifstream is("../../examples/Data/Plaza2_TD.txt");
|
||||
if (!is)
|
||||
throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
|
||||
|
||||
while (is) {
|
||||
double t, sender, receiver, range;
|
||||
is >> t >> sender >> receiver >> range;
|
||||
triples.push_back(RangeTriple(t, receiver, range));
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
return triples;
|
||||
}
|
||||
|
||||
// main
|
||||
int main (int argc, char** argv) {
|
||||
|
||||
// load Plaza2 data
|
||||
list<TimedOdometry> odometry = readOdometry();
|
||||
// size_t M = odometry.size();
|
||||
|
||||
vector<RangeTriple> triples = readTriples();
|
||||
size_t K = triples.size();
|
||||
|
||||
// parameters
|
||||
size_t minK = 150; // minimum number of range measurements to process initially
|
||||
size_t incK = 25; // minimum number of range measurements to process after
|
||||
bool groundTruth = false;
|
||||
bool robust = true;
|
||||
|
||||
// Set Noise parameters
|
||||
Vector priorSigmas = Vector3(1,1,M_PI);
|
||||
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
|
||||
double sigmaR = 100; // range standard deviation
|
||||
const NM::Base::shared_ptr // all same type
|
||||
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
|
||||
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
|
||||
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
|
||||
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
|
||||
rangeNoise = robust ? tukey : gaussian;
|
||||
|
||||
// Initialize iSAM
|
||||
ISAM2 isam;
|
||||
|
||||
// Add prior on first pose
|
||||
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
|
||||
M_PI - 2.02108900000000);
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
|
||||
Values initial;
|
||||
initial.insert(0, pose0);
|
||||
|
||||
// initialize points
|
||||
if (groundTruth) { // from TL file
|
||||
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
|
||||
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
|
||||
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
|
||||
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
|
||||
} else { // drawn from sigma=1 Gaussian in matlab version
|
||||
initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
|
||||
initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
|
||||
initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
|
||||
initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
|
||||
}
|
||||
|
||||
// set some loop variables
|
||||
size_t i = 1; // step counter
|
||||
size_t k = 0; // range measurement counter
|
||||
bool initialized = false;
|
||||
Pose2 lastPose = pose0;
|
||||
size_t countK = 0;
|
||||
|
||||
// Loop over odometry
|
||||
gttic_(iSAM);
|
||||
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
|
||||
//--------------------------------- odometry loop -----------------------------------------
|
||||
double t;
|
||||
Pose2 odometry;
|
||||
boost::tie(t, odometry) = timedOdometry;
|
||||
|
||||
// add odometry factor
|
||||
newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
|
||||
|
||||
// predict pose and add as initial estimate
|
||||
Pose2 predictedPose = lastPose.compose(odometry);
|
||||
lastPose = predictedPose;
|
||||
initial.insert(i, predictedPose);
|
||||
|
||||
// Check if there are range factors to be added
|
||||
while (k < K && t >= boost::get<0>(triples[k])) {
|
||||
size_t j = boost::get<1>(triples[k]);
|
||||
double range = boost::get<2>(triples[k]);
|
||||
newFactors.add(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
|
||||
k = k + 1;
|
||||
countK = countK + 1;
|
||||
}
|
||||
|
||||
// Check whether to update iSAM 2
|
||||
if ((k > minK) && (countK > incK)) {
|
||||
if (!initialized) { // Do a full optimize for first minK ranges
|
||||
gttic_(batchInitialization);
|
||||
LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
|
||||
initial = batchOptimizer.optimize();
|
||||
gttoc_(batchInitialization);
|
||||
initialized = true;
|
||||
}
|
||||
gttic_(update);
|
||||
isam.update(newFactors, initial);
|
||||
gttoc_(update);
|
||||
gttic_(calculateEstimate);
|
||||
Values result = isam.calculateEstimate();
|
||||
gttoc_(calculateEstimate);
|
||||
lastPose = result.at<Pose2>(i);
|
||||
newFactors = NonlinearFactorGraph();
|
||||
initial = Values();
|
||||
countK = 0;
|
||||
}
|
||||
i += 1;
|
||||
//--------------------------------- odometry loop -----------------------------------------
|
||||
} // BOOST_FOREACH
|
||||
gttoc_(iSAM);
|
||||
|
||||
// Print timings
|
||||
tictoc_print_();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
@ -36,13 +36,6 @@
|
|||
// Here we will use Symbols
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
// Here we will use Projection factors to model the camera's landmark observations.
|
||||
// Also, we will initialize the robot at some location using a Prior factor.
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
|
||||
// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
|
||||
// include iSAM2 here
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
|
@ -52,6 +45,13 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
// Here we will use Projection factors to model the camera's landmark observations.
|
||||
// Also, we will initialize the robot at some location using a Prior factor.
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
|
|
|
|||
261
gtsam.h
261
gtsam.h
|
|
@ -76,6 +76,18 @@
|
|||
* - When forward-declaring template instantiations, use the generated/typedefed name, e.g.
|
||||
* class gtsam::Class1Pose2;
|
||||
* class gtsam::MyInstantiatedClass;
|
||||
* Boost.serialization within Matlab:
|
||||
* - you need to mark classes as being serializable in the markup file (see this file for an example).
|
||||
* - There are two options currently, depending on the class. To "mark" a class as serializable,
|
||||
* add a function with a particular signature so that wrap will catch it.
|
||||
* - Add "void serialize()" to a class to create serialization functions for a class.
|
||||
* Adding this flag subsumes the serializable() flag below. Requirements:
|
||||
* - A default constructor must be publicly accessible
|
||||
* - Must not be an abstract base class
|
||||
* - The class must have an actual boost.serialization serialize() function.
|
||||
* - Add "void serializable()" to a class if you only want the class to be serialized as a
|
||||
* part of a container (such as noisemodel). This version does not require a publicly
|
||||
* accessible default constructor.
|
||||
*/
|
||||
|
||||
/**
|
||||
|
|
@ -200,6 +212,9 @@ virtual class LieVector : gtsam::Value {
|
|||
// Lie group
|
||||
static gtsam::LieVector Expmap(Vector v);
|
||||
static Vector Logmap(const gtsam::LieVector& p);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/base/LieMatrix.h>
|
||||
|
|
@ -229,6 +244,9 @@ virtual class LieMatrix : gtsam::Value {
|
|||
// Lie group
|
||||
static gtsam::LieMatrix Expmap(Vector v);
|
||||
static Vector Logmap(const gtsam::LieMatrix& p);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
|
|
@ -267,6 +285,9 @@ virtual class Point2 : gtsam::Value {
|
|||
Vector vector() const;
|
||||
double dist(const gtsam::Point2& p2) const;
|
||||
double norm() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class StereoPoint2 : gtsam::Value {
|
||||
|
|
@ -296,6 +317,9 @@ virtual class StereoPoint2 : gtsam::Value {
|
|||
|
||||
// Standard Interface
|
||||
Vector vector() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class Point3 : gtsam::Value {
|
||||
|
|
@ -329,6 +353,9 @@ virtual class Point3 : gtsam::Value {
|
|||
double x() const;
|
||||
double y() const;
|
||||
double z() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class Rot2 : gtsam::Value {
|
||||
|
|
@ -371,6 +398,9 @@ virtual class Rot2 : gtsam::Value {
|
|||
double c() const;
|
||||
double s() const;
|
||||
Matrix matrix() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class Rot3 : gtsam::Value {
|
||||
|
|
@ -424,11 +454,15 @@ virtual class Rot3 : gtsam::Value {
|
|||
double yaw() const;
|
||||
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
|
||||
Vector quaternion() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class Pose2 : gtsam::Value {
|
||||
// Standard Constructor
|
||||
Pose2();
|
||||
Pose2(const gtsam::Pose2& pose);
|
||||
Pose2(double x, double y, double theta);
|
||||
Pose2(double theta, const gtsam::Point2& t);
|
||||
Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
|
||||
|
|
@ -470,6 +504,9 @@ virtual class Pose2 : gtsam::Value {
|
|||
gtsam::Point2 translation() const;
|
||||
gtsam::Rot2 rotation() const;
|
||||
Matrix matrix() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class Pose3 : gtsam::Value {
|
||||
|
|
@ -518,6 +555,9 @@ virtual class Pose3 : gtsam::Value {
|
|||
gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& pose);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class Cal3_S2 : gtsam::Value {
|
||||
|
|
@ -551,6 +591,9 @@ virtual class Cal3_S2 : gtsam::Value {
|
|||
Vector vector() const;
|
||||
Matrix matrix() const;
|
||||
Matrix matrix_inverse() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
|
|
@ -585,6 +628,9 @@ virtual class Cal3DS2 : gtsam::Value {
|
|||
Vector vector() const;
|
||||
Vector k() const;
|
||||
//Matrix K() const; //FIXME: Uppercase
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
class Cal3_S2Stereo {
|
||||
|
|
@ -634,6 +680,9 @@ virtual class CalibratedCamera : gtsam::Value {
|
|||
// Standard Interface
|
||||
gtsam::Pose3 pose() const;
|
||||
double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class SimpleCamera : gtsam::Value {
|
||||
|
|
@ -669,6 +718,9 @@ virtual class SimpleCamera : gtsam::Value {
|
|||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& point);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
template<CALIBRATION = {gtsam::Cal3DS2}>
|
||||
|
|
@ -705,6 +757,9 @@ virtual class PinholeCamera : gtsam::Value {
|
|||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& point);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
|
|
@ -988,6 +1043,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
|
|||
Matrix R() const;
|
||||
bool equals(gtsam::noiseModel::Base& expected, double tol);
|
||||
void print(string s) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Diagonal : gtsam::noiseModel::Gaussian {
|
||||
|
|
@ -996,6 +1054,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
|
|||
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
|
||||
Matrix R() const;
|
||||
void print(string s) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Constrained : gtsam::noiseModel::Diagonal {
|
||||
|
|
@ -1010,6 +1071,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
|
|||
static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
|
||||
|
||||
gtsam::noiseModel::Constrained* unit() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Isotropic : gtsam::noiseModel::Diagonal {
|
||||
|
|
@ -1017,11 +1081,17 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
|
|||
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
|
||||
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
|
||||
void print(string s) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Unit : gtsam::noiseModel::Isotropic {
|
||||
static gtsam::noiseModel::Unit* Create(size_t dim);
|
||||
void print(string s) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
namespace mEstimator {
|
||||
|
|
@ -1032,24 +1102,36 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
|
|||
Null();
|
||||
void print(string s) const;
|
||||
static gtsam::noiseModel::mEstimator::Null* Create();
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
|
||||
Fair(double c);
|
||||
void print(string s) const;
|
||||
static gtsam::noiseModel::mEstimator::Fair* Create(double c);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
|
||||
Huber(double k);
|
||||
void print(string s) const;
|
||||
static gtsam::noiseModel::mEstimator::Huber* Create(double k);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
|
||||
Tukey(double k);
|
||||
void print(string s) const;
|
||||
static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
}///\namespace mEstimator
|
||||
|
|
@ -1058,6 +1140,9 @@ virtual class Robust : gtsam::noiseModel::Base {
|
|||
Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
|
||||
static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
|
||||
void print(string s) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
};
|
||||
|
||||
}///\namespace noiseModel
|
||||
|
|
@ -1112,6 +1197,9 @@ class VectorValues {
|
|||
bool hasSameStructure(const gtsam::VectorValues& other) const;
|
||||
double dot(const gtsam::VectorValues& V) const;
|
||||
double norm() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
class GaussianConditional {
|
||||
|
|
@ -1134,6 +1222,9 @@ class GaussianConditional {
|
|||
void solveInPlace(gtsam::VectorValues& x) const;
|
||||
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
|
||||
void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
class GaussianDensity {
|
||||
|
|
@ -1180,6 +1271,15 @@ virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase {
|
|||
GaussianBayesTree();
|
||||
GaussianBayesTree(const gtsam::GaussianBayesNet& bn);
|
||||
GaussianBayesTree(const gtsam::GaussianBayesNet& other);
|
||||
bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
|
||||
void print(string s);
|
||||
size_t size() const;
|
||||
size_t nrNodes() const;
|
||||
bool empty() const;
|
||||
gtsam::GaussianBayesTreeClique* root() const;
|
||||
gtsam::GaussianBayesTreeClique* clique(size_t j) const;
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
void saveGraph(string s) const;
|
||||
};
|
||||
|
||||
// namespace functions for GaussianBayesTree
|
||||
|
|
@ -1244,6 +1344,9 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
|
|||
void assertInvariants() const;
|
||||
|
||||
//gtsam::SharedDiagonal& get_model();
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class HessianFactor : gtsam::GaussianFactor {
|
||||
|
|
@ -1277,6 +1380,9 @@ virtual class HessianFactor : gtsam::GaussianFactor {
|
|||
void partialCholesky(size_t nrFrontals);
|
||||
gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals);
|
||||
void assertInvariants() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
class GaussianFactorGraph {
|
||||
|
|
@ -1323,6 +1429,9 @@ class GaussianFactorGraph {
|
|||
pair<Matrix,Vector> jacobian() const;
|
||||
Matrix augmentedHessian() const;
|
||||
pair<Matrix,Vector> hessian() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
//Non-Class functions in GaussianFactorGraph.h
|
||||
|
|
@ -1502,6 +1611,9 @@ class Ordering {
|
|||
void push_back(size_t key);
|
||||
void permuteInPlace(const gtsam::Permutation& permutation);
|
||||
void permuteInPlace(const gtsam::Permutation& selector, const gtsam::Permutation& permutation);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
class NonlinearFactorGraph {
|
||||
|
|
@ -1529,22 +1641,34 @@ class NonlinearFactorGraph {
|
|||
const gtsam::Ordering& ordering) const;
|
||||
gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const;
|
||||
gtsam::NonlinearFactorGraph clone() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class NonlinearFactor {
|
||||
// Factor base class
|
||||
size_t size() const;
|
||||
gtsam::KeyVector keys() const;
|
||||
void print(string s) const;
|
||||
void printKeys(string s) const;
|
||||
// NonlinearFactor
|
||||
void equals(const gtsam::NonlinearFactor& other, double tol) const;
|
||||
gtsam::KeyVector keys() const;
|
||||
size_t size() const;
|
||||
size_t dim() const;
|
||||
double error(const gtsam::Values& c) const;
|
||||
size_t dim() const;
|
||||
bool active(const gtsam::Values& c) const;
|
||||
gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;
|
||||
gtsam::NonlinearFactor* clone() const;
|
||||
// gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
|
||||
};
|
||||
|
||||
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
|
||||
void equals(const gtsam::NoiseModelFactor& other, double tol) const;
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
Vector unwhitenedError(const gtsam::Values& x) const;
|
||||
Vector whitenedError(const gtsam::Values& x) const;
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
class Values {
|
||||
Values();
|
||||
|
|
@ -1552,29 +1676,32 @@ class Values {
|
|||
|
||||
size_t size() const;
|
||||
bool empty() const;
|
||||
void clear();
|
||||
size_t dim() const;
|
||||
void clear();
|
||||
size_t dim() const;
|
||||
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::Values& other, double tol) const;
|
||||
|
||||
void insert(size_t j, const gtsam::Value& value);
|
||||
void insert(const gtsam::Values& values);
|
||||
void update(size_t j, const gtsam::Value& val);
|
||||
void update(const gtsam::Values& values);
|
||||
void erase(size_t j);
|
||||
void swap(gtsam::Values& values);
|
||||
void insert(const gtsam::Values& values);
|
||||
void update(size_t j, const gtsam::Value& val);
|
||||
void update(const gtsam::Values& values);
|
||||
void erase(size_t j);
|
||||
void swap(gtsam::Values& values);
|
||||
|
||||
bool exists(size_t j) const;
|
||||
gtsam::Value at(size_t j) const;
|
||||
gtsam::KeyList keys() const;
|
||||
|
||||
gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const;
|
||||
gtsam::Ordering* orderingArbitrary(size_t firstVar) const;
|
||||
gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const;
|
||||
gtsam::Ordering* orderingArbitrary(size_t firstVar) const;
|
||||
|
||||
gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const;
|
||||
gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const;
|
||||
void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const;
|
||||
gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const;
|
||||
gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const;
|
||||
void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
// Actually a FastList<Key>
|
||||
|
|
@ -1691,6 +1818,8 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
|
|||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::Ordering& ordering);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serializable() const;
|
||||
}; // \class LinearContainerFactor
|
||||
|
||||
// Summarization functionality
|
||||
|
|
@ -1961,37 +2090,43 @@ class NonlinearISAM {
|
|||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
|
||||
virtual class PriorFactor : gtsam::NonlinearFactor {
|
||||
virtual class PriorFactor : gtsam::NoiseModelFactor {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
T prior() const;
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
|
||||
virtual class BetweenFactor : gtsam::NonlinearFactor {
|
||||
virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
||||
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
|
||||
T measured() const;
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
|
||||
virtual class NonlinearEquality : gtsam::NonlinearFactor {
|
||||
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
|
||||
// Constructor - forces exact evaluation
|
||||
NonlinearEquality(size_t j, const T& feasible);
|
||||
// Constructor - allows inexact evaluation
|
||||
NonlinearEquality(size_t j, const T& feasible, double error_gain);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam/slam/RangeFactor.h>
|
||||
template<POSE, POINT>
|
||||
virtual class RangeFactor : gtsam::NonlinearFactor {
|
||||
virtual class RangeFactor : gtsam::NoiseModelFactor {
|
||||
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
};
|
||||
|
||||
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
|
||||
|
|
@ -2006,9 +2141,11 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactor
|
|||
|
||||
#include <gtsam/slam/BearingFactor.h>
|
||||
template<POSE, POINT, ROTATION>
|
||||
virtual class BearingFactor : gtsam::NonlinearFactor {
|
||||
virtual class BearingFactor : gtsam::NoiseModelFactor {
|
||||
BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
|
||||
|
|
@ -2016,9 +2153,11 @@ typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFa
|
|||
|
||||
#include <gtsam/slam/BearingRangeFactor.h>
|
||||
template<POSE, POINT, ROTATION>
|
||||
virtual class BearingRangeFactor : gtsam::NonlinearFactor {
|
||||
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
|
||||
BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
|
||||
|
|
@ -2026,7 +2165,7 @@ typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> Bear
|
|||
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
template<POSE, LANDMARK, CALIBRATION>
|
||||
virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
|
||||
virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
|
||||
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
||||
size_t poseKey, size_t pointKey, const CALIBRATION* k);
|
||||
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
|
||||
|
|
@ -2042,7 +2181,9 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
|
|||
CALIBRATION* calibration() const;
|
||||
bool verboseCheirality() const;
|
||||
bool throwCheirality() const;
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
|
||||
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
|
||||
|
|
@ -2050,7 +2191,7 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3D
|
|||
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
template<CAMERA, LANDMARK>
|
||||
virtual class GeneralSFMFactor : gtsam::NonlinearFactor {
|
||||
virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
||||
GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
|
||||
gtsam::Point2 measured() const;
|
||||
};
|
||||
|
|
@ -2058,28 +2199,32 @@ typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFa
|
|||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
|
||||
template<CALIBRATION = {gtsam::Cal3_S2}>
|
||||
virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor {
|
||||
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
||||
GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
|
||||
gtsam::Point2 measured() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam/slam/StereoFactor.h>
|
||||
template<POSE, LANDMARK>
|
||||
virtual class GenericStereoFactor : gtsam::NonlinearFactor {
|
||||
virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
|
||||
GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel,
|
||||
size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
|
||||
gtsam::StereoPoint2 measured() const;
|
||||
gtsam::Cal3_S2Stereo* calibration() const;
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
|
||||
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
template<POSE>
|
||||
virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
|
||||
virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
|
||||
PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
};
|
||||
|
||||
typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
|
||||
|
|
@ -2087,9 +2232,8 @@ typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
|
|||
|
||||
#include <gtsam/slam/PoseRotationPrior.h>
|
||||
template<POSE>
|
||||
virtual class PoseRotationPrior : gtsam::NonlinearFactor {
|
||||
virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
|
||||
PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
|
||||
gtsam::noiseModel::Base* get_noiseModel() const;
|
||||
};
|
||||
|
||||
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
|
||||
|
|
@ -2107,51 +2251,6 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
|
|||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
|
||||
gtsam::noiseModel::Base* model);
|
||||
|
||||
//*************************************************************************
|
||||
// Serialization
|
||||
//*************************************************************************
|
||||
#include <gtsam/slam/serialization.h>
|
||||
|
||||
// Serialize/Deserialize a NonlinearFactorGraph
|
||||
string serializeGraph(const gtsam::NonlinearFactorGraph& graph);
|
||||
|
||||
gtsam::NonlinearFactorGraph* deserializeGraph(string serialized_graph);
|
||||
|
||||
string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph);
|
||||
string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph, string name);
|
||||
|
||||
gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph);
|
||||
gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph, string name);
|
||||
|
||||
// Serialize/Deserialize a Values
|
||||
string serializeValues(const gtsam::Values& values);
|
||||
|
||||
gtsam::Values* deserializeValues(string serialized_values);
|
||||
|
||||
string serializeValuesXML(const gtsam::Values& values);
|
||||
string serializeValuesXML(const gtsam::Values& values, string name);
|
||||
|
||||
gtsam::Values* deserializeValuesXML(string serialized_values);
|
||||
gtsam::Values* deserializeValuesXML(string serialized_values, string name);
|
||||
|
||||
// Serialize
|
||||
bool serializeGraphToFile(const gtsam::NonlinearFactorGraph& graph, string fname);
|
||||
bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname);
|
||||
bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname, string name);
|
||||
|
||||
bool serializeValuesToFile(const gtsam::Values& values, string fname);
|
||||
bool serializeValuesToXMLFile(const gtsam::Values& values, string fname);
|
||||
bool serializeValuesToXMLFile(const gtsam::Values& values, string fname, string name);
|
||||
|
||||
// Deserialize
|
||||
gtsam::NonlinearFactorGraph* deserializeGraphFromFile(string fname);
|
||||
gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname);
|
||||
gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname, string name);
|
||||
|
||||
gtsam::Values* deserializeValuesFromFile(string fname);
|
||||
gtsam::Values* deserializeValuesFromXMLFile(string fname);
|
||||
gtsam::Values* deserializeValuesFromXMLFile(string fname, string name);
|
||||
|
||||
//*************************************************************************
|
||||
// Utilities
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -33,11 +33,13 @@ endif()
|
|||
# To exclude a source from the library build (in any subfolder)
|
||||
# Add the full name to this list, as in the following example
|
||||
# Sources to remove from builds
|
||||
set (excluded_sources "")
|
||||
# "${CMAKE_CURRENT_SOURCE_DIR}/discrete/TypedDiscreteFactor.cpp"
|
||||
# "${CMAKE_CURRENT_SOURCE_DIR}/discrete/TypedDiscreteFactorGraph.cpp"
|
||||
# "${CMAKE_CURRENT_SOURCE_DIR}/discrete/parseUAI.cpp"
|
||||
# "${CMAKE_CURRENT_SOURCE_DIR}/discrete/PotentialTable.cpp")
|
||||
set (excluded_sources #"")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
|
||||
)
|
||||
|
||||
set (excluded_headers #"")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
|
||||
)
|
||||
|
||||
if(GTSAM_USE_QUATERNIONS)
|
||||
set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3M.cpp")
|
||||
|
|
@ -54,9 +56,10 @@ foreach(subdir ${gtsam_subdirs})
|
|||
# Build convenience libraries
|
||||
file(GLOB subdir_cpp_srcs "${subdir}/*.cpp")
|
||||
file(GLOB subdir_headers "${subdir}/*.h")
|
||||
list(REMOVE_ITEM subdir_cpp_srcs ${excluded_sources})
|
||||
list(REMOVE_ITEM subdir_headers ${excluded_headers})
|
||||
set(subdir_srcs ${subdir_cpp_srcs} ${subdir_headers}) # Include header files so they show up in Visual Studio
|
||||
gtsam_assign_source_folders("${subdir_srcs}") # Create MSVC structure
|
||||
list(REMOVE_ITEM subdir_srcs ${excluded_sources})
|
||||
set(${subdir}_srcs ${subdir_srcs})
|
||||
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES AND (subdir_cpp_srcs)) # Only build convenience library if sources exist
|
||||
message(STATUS "Building Convenience Library: ${subdir}")
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ namespace gtsam {
|
|||
typedef Eigen::VectorXd Vector;
|
||||
|
||||
// Commonly used fixed size vectors
|
||||
typedef Eigen::Vector2d Vector2;
|
||||
typedef Eigen::Vector3d Vector3;
|
||||
typedef Eigen::Matrix<double, 6, 1> Vector6;
|
||||
|
||||
|
|
@ -373,7 +374,21 @@ namespace boost {
|
|||
ar >> make_nvp("data", make_array(v.data(), v.size()));
|
||||
}
|
||||
|
||||
// split version - copies into an STL vector for serialization
|
||||
template<class Archive, int D>
|
||||
void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int version) {
|
||||
ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
|
||||
}
|
||||
|
||||
template<class Archive, int D>
|
||||
void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int version) {
|
||||
ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
|
||||
}
|
||||
|
||||
} // namespace serialization
|
||||
} // namespace boost
|
||||
|
||||
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
|
||||
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
|
||||
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
|
||||
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)
|
||||
|
|
|
|||
|
|
@ -85,12 +85,21 @@ TEST (Serialization, FastVector) {
|
|||
/* ************************************************************************* */
|
||||
TEST (Serialization, matrix_vector) {
|
||||
EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equality<Vector2>(Vector2(1.0, 2.0)));
|
||||
EXPECT(equality<Vector3>(Vector3(1.0, 2.0, 3.0)));
|
||||
EXPECT(equality<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
|
||||
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
|
||||
|
||||
EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equalityXML<Vector2>(Vector2(1.0, 2.0)));
|
||||
EXPECT(equalityXML<Vector3>(Vector3(1.0, 2.0, 3.0)));
|
||||
EXPECT(equalityXML<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
|
||||
EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
|
||||
|
||||
EXPECT(equalityBinary<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
|
||||
EXPECT(equalityBinary<Vector2>(Vector2(1.0, 2.0)));
|
||||
EXPECT(equalityBinary<Vector3>(Vector3(1.0, 2.0, 3.0)));
|
||||
EXPECT(equalityBinary<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
|
||||
EXPECT(equalityBinary<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/base/Lie-inl.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -25,6 +27,8 @@ namespace gtsam {
|
|||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Point2);
|
||||
|
||||
static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Point2::print(const string& s) const {
|
||||
cout << s << *this << endl;
|
||||
|
|
@ -36,8 +40,107 @@ bool Point2::equals(const Point2& q, double tol) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point2::norm() const {
|
||||
return sqrt(x_*x_ + y_*y_);
|
||||
double Point2::norm(boost::optional<Matrix&> H) const {
|
||||
double r = sqrt(x_ * x_ + y_ * y_);
|
||||
if (H) {
|
||||
Matrix D_result_d;
|
||||
if (fabs(r) > 1e-10)
|
||||
D_result_d = Matrix_(1, 2, x_ / r, y_ / r);
|
||||
else
|
||||
D_result_d = oneOne; // TODO: really infinity, why 1 here??
|
||||
*H = D_result_d;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static const Matrix I2 = eye(2);
|
||||
double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Point2 d = point - *this;
|
||||
if (H1 || H2) {
|
||||
Matrix H;
|
||||
double r = d.norm(H);
|
||||
if (H1) *H1 = -H;
|
||||
if (H2) *H2 = H;
|
||||
return r;
|
||||
} else
|
||||
return d.norm();
|
||||
}
|
||||
|
||||
/*
|
||||
* Calculate f and h, respectively the parallel and perpendicular distance of
|
||||
* the intersections of two circles along and from the line connecting the centers.
|
||||
* Both are dimensionless fractions of the distance d between the circle centers.
|
||||
* If the circles do not intersect or they are identical, returns boost::none.
|
||||
* If one solution (touching circles, as determined by tol), h will be exactly zero.
|
||||
* h is a good measure for how accurate the intersection will be, as when circles touch
|
||||
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
|
||||
* @param R_d : R/d, ratio of radius of first circle to distance between centers
|
||||
* @param r_d : r/d, ratio of radius of second circle to distance between centers
|
||||
* @param tol: absolute tolerance below which we consider touching circles
|
||||
*/
|
||||
/* ************************************************************************* */
|
||||
// Math inspired by http://paulbourke.net/geometry/circlesphere/
|
||||
boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
|
||||
double tol) {
|
||||
|
||||
double R2_d2 = R_d*R_d; // Yes, RD-D2 !
|
||||
double f = 0.5 + 0.5*(R2_d2 - r_d*r_d);
|
||||
double h2 = R2_d2 - f*f; // just right triangle rule
|
||||
|
||||
// h^2<0 is equivalent to (d > (R + r) || d < (R - r))
|
||||
// Hence, there are only solutions if >=0
|
||||
if (h2<-tol) return boost::none; // allow *slightly* negative
|
||||
else if (h2<tol) return Point2(f,0.0); // one solution
|
||||
else return Point2(f,sqrt(h2)); // two solutions
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
|
||||
boost::optional<Point2> fh) {
|
||||
|
||||
list<Point2> solutions;
|
||||
// If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r)
|
||||
if (fh) {
|
||||
// vector between circle centers
|
||||
Point2 c12 = c2-c1;
|
||||
|
||||
// Determine p2, the point where the line through the circle
|
||||
// intersection points crosses the line between the circle centers.
|
||||
Point2 p2 = c1 + fh->x() * c12;
|
||||
|
||||
// If h == 0, the circles are touching, so just return one point
|
||||
if (fh->y()==0.0)
|
||||
solutions.push_back(p2);
|
||||
else {
|
||||
// determine the offsets of the intersection points from p
|
||||
Point2 offset = fh->y() * Point2(-c12.y(), c12.x());
|
||||
|
||||
// Determine the absolute intersection points.
|
||||
solutions.push_back(p2 + offset);
|
||||
solutions.push_back(p2 - offset);
|
||||
}
|
||||
}
|
||||
return solutions;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2,
|
||||
double r2, double tol) {
|
||||
|
||||
// distance between circle centers.
|
||||
double d = c1.dist(c2);
|
||||
|
||||
// centers coincide, either no solution or infinite number of solutions.
|
||||
if (d<1e-9) return list<Point2>();
|
||||
|
||||
// Calculate f and h given normalized radii
|
||||
double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
|
||||
boost::optional<Point2> fh = CircleCircleIntersection(R_d,r_d);
|
||||
|
||||
// Call version that takes fh
|
||||
return CircleCircleIntersection(c1, c2, fh);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -65,6 +65,43 @@ public:
|
|||
y_ = v(1);
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Circle-circle intersection, given normalized radii.
|
||||
* Calculate f and h, respectively the parallel and perpendicular distance of
|
||||
* the intersections of two circles along and from the line connecting the centers.
|
||||
* Both are dimensionless fractions of the distance d between the circle centers.
|
||||
* If the circles do not intersect or they are identical, returns boost::none.
|
||||
* If one solution (touching circles, as determined by tol), h will be exactly zero.
|
||||
* h is a good measure for how accurate the intersection will be, as when circles touch
|
||||
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
|
||||
* @param R_d : R/d, ratio of radius of first circle to distance between centers
|
||||
* @param r_d : r/d, ratio of radius of second circle to distance between centers
|
||||
* @param tol: absolute tolerance below which we consider touching circles
|
||||
* @return optional Point2 with f and h, boost::none if no solution.
|
||||
*/
|
||||
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d,
|
||||
double tol = 1e-9);
|
||||
|
||||
/*
|
||||
* @brief Circle-circle intersection, from the normalized radii solution.
|
||||
* @param c1 center of first circle
|
||||
* @param c2 center of second circle
|
||||
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
|
||||
*/
|
||||
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>);
|
||||
|
||||
/**
|
||||
* @brief Intersect 2 circles
|
||||
* @param c1 center of first circle
|
||||
* @param r1 radius of first circle
|
||||
* @param c2 center of second circle
|
||||
* @param r2 radius of second circle
|
||||
* @param tol: absolute tolerance below which we consider touching circles
|
||||
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
|
||||
*/
|
||||
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1,
|
||||
Point2 c2, double r2, double tol = 1e-9);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
@ -144,16 +181,15 @@ public:
|
|||
/// @name Vector Space
|
||||
/// @{
|
||||
|
||||
/** norm of point */
|
||||
double norm() const;
|
||||
|
||||
/** creates a unit vector */
|
||||
Point2 unit() const { return *this/norm(); }
|
||||
|
||||
/** norm of point */
|
||||
double norm(boost::optional<Matrix&> H = boost::none) const;
|
||||
|
||||
/** distance between two points */
|
||||
inline double distance(const Point2& p2) const {
|
||||
return (p2 - *this).norm();
|
||||
}
|
||||
double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/** @deprecated The following function has been deprecated, use distance above */
|
||||
inline double dist(const Point2& p2) const {
|
||||
|
|
@ -180,7 +216,7 @@ public:
|
|||
double y() const {return y_;}
|
||||
|
||||
/// return vectorized form (column-wise). TODO: why does this function exist?
|
||||
Vector vector() const { return Vector_(2, x_, y_); }
|
||||
Vector2 vector() const { return Vector2(x_, y_); }
|
||||
|
||||
/// @}
|
||||
/// @name Deprecated (non-const, non-functional style. Do not use).
|
||||
|
|
@ -190,7 +226,7 @@ public:
|
|||
/// @}
|
||||
|
||||
/// Streaming
|
||||
friend std::ostream &operator<<(std::ostream &os, const Point2& p);
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -180,9 +180,7 @@ namespace gtsam {
|
|||
bool operator ==(const Point3& q) const;
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
Vector3 vector() const {
|
||||
return Vector3(x_,y_,z_);
|
||||
}
|
||||
Vector3 vector() const { return Vector3(x_,y_,z_); }
|
||||
|
||||
/// get x
|
||||
inline double x() const {return x_;}
|
||||
|
|
@ -204,7 +202,7 @@ namespace gtsam {
|
|||
/// @}
|
||||
|
||||
/// Output stream operator
|
||||
friend std::ostream &operator<<(std::ostream &os, const Point3& p);
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -221,32 +221,31 @@ Rot2 Pose2::bearing(const Pose2& point,
|
|||
/* ************************************************************************* */
|
||||
double Pose2::range(const Point2& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2) return transform_to(point).norm();
|
||||
Point2 d = transform_to(point, H1, H2);
|
||||
double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
|
||||
Matrix D_result_d;
|
||||
if(std::abs(r) > 1e-10)
|
||||
D_result_d = Matrix_(1, 2, x / r, y / r);
|
||||
else {
|
||||
D_result_d = Matrix_(1,2, 1.0, 1.0);
|
||||
}
|
||||
if (H1) *H1 = D_result_d * (*H1);
|
||||
if (H2) *H2 = D_result_d * (*H2);
|
||||
Point2 d = point - t_;
|
||||
if (!H1 && !H2) return d.norm();
|
||||
Matrix H;
|
||||
double r = d.norm(H);
|
||||
if (H1) *H1 = H * Matrix_(2, 3,
|
||||
-r_.c(), r_.s(), 0.0,
|
||||
-r_.s(), -r_.c(), 0.0);
|
||||
if (H2) *H2 = H;
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose2::range(const Pose2& point,
|
||||
double Pose2::range(const Pose2& pose2,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
double r = range(point.t(), H1, H2);
|
||||
if (H2) {
|
||||
// NOTE: expmap changes the orientation of expmap direction,
|
||||
// so we must rotate the jacobian
|
||||
Matrix H2_ = *H2 * point.r().matrix();
|
||||
*H2 = zeros(1, 3);
|
||||
insertSub(*H2, H2_, 0, 0);
|
||||
}
|
||||
Point2 d = pose2.t() - t_;
|
||||
if (!H1 && !H2) return d.norm();
|
||||
Matrix H;
|
||||
double r = d.norm(H);
|
||||
if (H1) *H1 = H * Matrix_(2, 3,
|
||||
-r_.c(), r_.s(), 0.0,
|
||||
-r_.s(), -r_.c(), 0.0);
|
||||
if (H2) *H2 = H * Matrix_(2, 3,
|
||||
pose2.r_.c(), -pose2.r_.s(), 0.0,
|
||||
pose2.r_.s(), pose2.r_.c(), 0.0);
|
||||
return r;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -291,7 +291,7 @@ namespace gtsam {
|
|||
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }
|
||||
|
||||
/// Output stream operator
|
||||
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -356,7 +356,7 @@ namespace gtsam {
|
|||
Vector quaternion() const;
|
||||
|
||||
/// Output stream operator
|
||||
friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -15,9 +15,11 @@
|
|||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -27,13 +29,13 @@ GTSAM_CONCEPT_LIE_INST(Point2)
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Point2, constructor) {
|
||||
Point2 p1(1,2), p2 = p1;
|
||||
Point2 p1(1, 2), p2 = p1;
|
||||
EXPECT(assert_equal(p1, p2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Point2, Lie) {
|
||||
Point2 p1(1,2), p2(4,5);
|
||||
Point2 p1(1, 2), p2(4, 5);
|
||||
Matrix H1, H2;
|
||||
|
||||
EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2)));
|
||||
|
|
@ -49,8 +51,7 @@ TEST(Point2, Lie) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, expmap)
|
||||
{
|
||||
TEST( Point2, expmap) {
|
||||
Vector d(2);
|
||||
d(0) = 1;
|
||||
d(1) = -1;
|
||||
|
|
@ -59,8 +60,7 @@ TEST( Point2, expmap)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, arithmetic)
|
||||
{
|
||||
TEST( Point2, arithmetic) {
|
||||
EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) ));
|
||||
EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1)));
|
||||
EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) ));
|
||||
|
|
@ -70,34 +70,138 @@ TEST( Point2, arithmetic)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, norm)
|
||||
{
|
||||
Point2 p0(cos(5.0), sin(5.0));
|
||||
DOUBLES_EQUAL(1,p0.norm(),1e-6);
|
||||
Point2 p1(4, 5), p2(1, 1);
|
||||
DOUBLES_EQUAL( 5,p1.distance(p2),1e-6);
|
||||
DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, unit)
|
||||
{
|
||||
Point2 p0(10, 0), p1(0,-10), p2(10, 10);
|
||||
TEST( Point2, unit) {
|
||||
Point2 p0(10, 0), p1(0, -10), p2(10, 10);
|
||||
EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, stream)
|
||||
{
|
||||
Point2 p(1,2);
|
||||
// some shared test values
|
||||
Point2 x1, x2(1, 1), x3(1, 1);
|
||||
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
LieVector norm_proxy(const Point2& point) {
|
||||
return LieVector(point.norm());
|
||||
}
|
||||
TEST( Point2, norm ) {
|
||||
Point2 p0(cos(5.0), sin(5.0));
|
||||
DOUBLES_EQUAL(1, p0.norm(), 1e-6);
|
||||
Point2 p1(4, 5), p2(1, 1);
|
||||
DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6);
|
||||
DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
|
||||
|
||||
Matrix expectedH, actualH;
|
||||
double actual;
|
||||
|
||||
// exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
|
||||
actual = x1.norm(actualH);
|
||||
EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
|
||||
expectedH = Matrix_(1, 2, 1.0, 1.0);
|
||||
EXPECT(assert_equal(expectedH,actualH));
|
||||
|
||||
actual = x2.norm(actualH);
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2), actual, 1e-9);
|
||||
expectedH = numericalDerivative11(norm_proxy, x2);
|
||||
EXPECT(assert_equal(expectedH,actualH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LieVector distance_proxy(const Point2& location, const Point2& point) {
|
||||
return LieVector(location.distance(point));
|
||||
}
|
||||
TEST( Point2, distance ) {
|
||||
Matrix expectedH1, actualH1, expectedH2, actualH2;
|
||||
|
||||
// establish distance is indeed zero
|
||||
EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9);
|
||||
|
||||
// establish distance is indeed 45 degrees
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9);
|
||||
|
||||
// Another pair
|
||||
double actual23 = x2.distance(l3, actualH1, actualH2);
|
||||
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(distance_proxy, x2, l3);
|
||||
expectedH2 = numericalDerivative22(distance_proxy, x2, l3);
|
||||
EXPECT(assert_equal(expectedH1,actualH1));
|
||||
EXPECT(assert_equal(expectedH2,actualH2));
|
||||
|
||||
// Another test
|
||||
double actual34 = x3.distance(l4, actualH1, actualH2);
|
||||
EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
|
||||
|
||||
// Check numerical derivatives
|
||||
expectedH1 = numericalDerivative21(distance_proxy, x3, l4);
|
||||
expectedH2 = numericalDerivative22(distance_proxy, x3, l4);
|
||||
EXPECT(assert_equal(expectedH1,actualH1));
|
||||
EXPECT(assert_equal(expectedH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, circleCircleIntersection) {
|
||||
|
||||
double offset = 0.994987;
|
||||
// Test intersections of circle moving from inside to outside
|
||||
|
||||
list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
|
||||
EXPECT_LONGS_EQUAL(0,inside.size());
|
||||
|
||||
list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
|
||||
EXPECT_LONGS_EQUAL(1,touching1.size());
|
||||
EXPECT(assert_equal(Point2(5,0), touching1.front()));
|
||||
|
||||
list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
|
||||
EXPECT_LONGS_EQUAL(2,common.size());
|
||||
EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
|
||||
|
||||
list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
|
||||
EXPECT_LONGS_EQUAL(1,touching2.size());
|
||||
EXPECT(assert_equal(Point2(5,0), touching2.front()));
|
||||
|
||||
// test rotated case
|
||||
list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
|
||||
EXPECT_LONGS_EQUAL(2,rotated.size());
|
||||
EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
|
||||
|
||||
// test r1<r2
|
||||
list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
|
||||
EXPECT_LONGS_EQUAL(2,smaller.size());
|
||||
EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
|
||||
|
||||
// test offset case, r1>r2
|
||||
list<Point2> offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
|
||||
EXPECT_LONGS_EQUAL(2,offset1.size());
|
||||
EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
|
||||
|
||||
// test offset case, r1<r2
|
||||
list<Point2> offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
|
||||
EXPECT_LONGS_EQUAL(2,offset2.size());
|
||||
EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Point2, stream) {
|
||||
Point2 p(1, 2);
|
||||
std::ostringstream os;
|
||||
os << p;
|
||||
EXPECT(os.str() == "(1, 2)");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
int main () {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -14,22 +14,21 @@
|
|||
* @brief Unit tests for Pose2 class
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
|
|
|
|||
|
|
@ -161,6 +161,16 @@ protected:
|
|||
template<class FG> void fill(const FG& factorGraph);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(index_);
|
||||
ar & BOOST_SERIALIZATION_NVP(nFactors_);
|
||||
ar & BOOST_SERIALIZATION_NVP(nEntries_);
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -22,6 +22,11 @@ if (GTSAM_BUILD_TESTS)
|
|||
gtsam_add_subdir_tests(linear "${linear_local_libs}" "${gtsam-default}" "${linear_excluded_files}")
|
||||
endif(GTSAM_BUILD_TESTS)
|
||||
|
||||
if(MSVC)
|
||||
set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerializationLinear.cpp"
|
||||
APPEND PROPERTY COMPILE_FLAGS "/bigobj")
|
||||
endif()
|
||||
|
||||
# Build timing scripts
|
||||
if (GTSAM_BUILD_TIMING)
|
||||
gtsam_add_subdir_timing(linear "${linear_local_libs}" "${gtsam-default}" "${linear_excluded_files}")
|
||||
|
|
|
|||
|
|
@ -27,12 +27,12 @@
|
|||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
// Forward declaration to friend unit tests
|
||||
class eliminate2JacobianFactorTest;
|
||||
class constructorGaussianConditionalTest;
|
||||
class eliminationGaussianFactorGraphTest;
|
||||
class complicatedMarginalGaussianJunctionTreeTest;
|
||||
class informationGaussianConditionalTest;
|
||||
class isGaussianFactorGaussianConditionalTest;
|
||||
class JacobianFactoreliminate2Test;
|
||||
class GaussianConditionalconstructorTest;
|
||||
class GaussianFactorGrapheliminationTest;
|
||||
class GaussianJunctionTreecomplicatedMarginalTest;
|
||||
class GaussianConditionalinformationTest;
|
||||
class GaussianConditionalisGaussianFactorTest;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -223,12 +223,12 @@ private:
|
|||
|
||||
// Friends
|
||||
friend class JacobianFactor;
|
||||
friend class ::eliminate2JacobianFactorTest;
|
||||
friend class ::constructorGaussianConditionalTest;
|
||||
friend class ::eliminationGaussianFactorGraphTest;
|
||||
friend class ::complicatedMarginalGaussianJunctionTreeTest;
|
||||
friend class ::informationGaussianConditionalTest;
|
||||
friend class ::isGaussianFactorGaussianConditionalTest;
|
||||
friend class ::JacobianFactoreliminate2Test;
|
||||
friend class ::GaussianConditionalconstructorTest;
|
||||
friend class ::GaussianFactorGrapheliminationTest;
|
||||
friend class ::GaussianJunctionTreecomplicatedMarginalTest;
|
||||
friend class ::GaussianConditionalinformationTest;
|
||||
friend class ::GaussianConditionalisGaussianFactorTest;
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -23,10 +23,10 @@
|
|||
#include <gtsam/linear/GaussianFactor.h>
|
||||
|
||||
// Forward declarations for friend unit tests
|
||||
class ConversionConstructorHessianFactorTest;
|
||||
class Constructor1HessianFactorTest;
|
||||
class Constructor1bHessianFactorTest;
|
||||
class combineHessianFactorTest;
|
||||
class HessianFactorConversionConstructorTest;
|
||||
class HessianFactorConstructor1Test;
|
||||
class HessianFactorConstructor1bTest;
|
||||
class HessianFactorcombineTest;
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -325,10 +325,10 @@ namespace gtsam {
|
|||
virtual Matrix information() const;
|
||||
|
||||
// Friend unit test classes
|
||||
friend class ::ConversionConstructorHessianFactorTest;
|
||||
friend class ::Constructor1HessianFactorTest;
|
||||
friend class ::Constructor1bHessianFactorTest;
|
||||
friend class ::combineHessianFactorTest;
|
||||
friend class ::HessianFactorConversionConstructorTest;
|
||||
friend class ::HessianFactorConstructor1Test;
|
||||
friend class ::HessianFactorConstructor1bTest;
|
||||
friend class ::HessianFactorcombineTest;
|
||||
|
||||
// Friend JacobianFactor for conversion
|
||||
friend class JacobianFactor;
|
||||
|
|
|
|||
|
|
@ -27,9 +27,9 @@
|
|||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
// Forward declarations of friend unit tests
|
||||
class Combine2JacobianFactorTest;
|
||||
class eliminateFrontalsJacobianFactorTest;
|
||||
class constructor2JacobianFactorTest;
|
||||
class JacobianFactorCombine2Test;
|
||||
class JacobianFactoreliminateFrontalsTest;
|
||||
class JacobianFactorconstructor2Test;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -316,9 +316,9 @@ namespace gtsam {
|
|||
friend class HessianFactor;
|
||||
|
||||
// Friend unit tests (see also forward declarations above)
|
||||
friend class ::Combine2JacobianFactorTest;
|
||||
friend class ::eliminateFrontalsJacobianFactorTest;
|
||||
friend class ::constructor2JacobianFactorTest;
|
||||
friend class ::JacobianFactorCombine2Test;
|
||||
friend class ::JacobianFactoreliminateFrontalsTest;
|
||||
friend class ::JacobianFactorconstructor2Test;
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -1043,7 +1043,7 @@ VectorValues optimize(const ISAM2& isam) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
|
||||
// We may need to update the solution calcaulations
|
||||
// We may need to update the solution calculations
|
||||
if(!isam.deltaDoglegUptodate_) {
|
||||
gttic(UpdateDoglegDeltas);
|
||||
double wildfireThreshold = 0.0;
|
||||
|
|
|
|||
|
|
@ -586,7 +586,7 @@ public:
|
|||
VALUE calculateEstimate(Key key) const;
|
||||
|
||||
/// @name Public members for non-typical usage
|
||||
//@{
|
||||
/// @{
|
||||
|
||||
/** Internal implementation functions */
|
||||
struct Impl;
|
||||
|
|
@ -619,7 +619,7 @@ public:
|
|||
/** prints out clique statistics */
|
||||
GTSAM_EXPORT void printStats() const { getCliqueData().getStats().print(); }
|
||||
|
||||
//@}
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,9 @@ protected:
|
|||
GaussianFactor::shared_ptr factor_;
|
||||
boost::optional<Values> linearizationPoint_;
|
||||
|
||||
/** Default constructor - necessary for serialization */
|
||||
LinearContainerFactor() {}
|
||||
|
||||
/** direct copy constructor */
|
||||
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
|
||||
const boost::optional<Values>& linearizationPoint);
|
||||
|
|
@ -148,6 +151,18 @@ protected:
|
|||
void rekeyFactor(const Ordering& ordering);
|
||||
void initializeLinearizationPoint(const Values& linearizationPoint);
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NonlinearFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(factor_);
|
||||
ar & BOOST_SERIALIZATION_NVP(linearizationPoint_);
|
||||
}
|
||||
|
||||
}; // \class LinearContainerFactor
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
const double logSqrt2PI = log(sqrt(2.0 * M_PI)); ///< constant needed below
|
||||
const double logSqrt2PI = log(std::sqrt(2.0 * M_PI)); ///< constant needed below
|
||||
|
||||
/**
|
||||
* @brief Binary factor to estimate parameters of zero-mean Gaussian white noise
|
||||
|
|
@ -32,22 +32,21 @@ namespace gtsam {
|
|||
* This factor uses the mean-precision parameterization.
|
||||
*
|
||||
* Takes three template arguments:
|
||||
* MKEY: key type to use for mean
|
||||
* PKEY: key type to use for precision
|
||||
* VALUES: Values type for optimization
|
||||
* Key: key type to use for mean
|
||||
* Key: key type to use for precision
|
||||
* Values: Values type for optimization
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class MKEY, class PKEY, class VALUES>
|
||||
class WhiteNoiseFactor: public NonlinearFactor<VALUES> {
|
||||
class WhiteNoiseFactor: public NonlinearFactor {
|
||||
|
||||
private:
|
||||
|
||||
double z_; ///< Measurement
|
||||
|
||||
MKEY meanKey_; ///< key by which to access mean variable
|
||||
PKEY precisionKey_; ///< key by which to access precision variable
|
||||
Key meanKey_; ///< key by which to access mean variable
|
||||
Key precisionKey_; ///< key by which to access precision variable
|
||||
|
||||
typedef NonlinearFactor<VALUES> Base;
|
||||
typedef NonlinearFactor Base;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -93,7 +92,7 @@ namespace gtsam {
|
|||
* @param meanKey Key for mean variable
|
||||
* @param precisionKey Key for precision variable
|
||||
*/
|
||||
WhiteNoiseFactor(double z, const MKEY& meanKey, const PKEY& precisionKey) :
|
||||
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
|
||||
Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
|
||||
}
|
||||
|
||||
|
|
@ -110,8 +109,9 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// Print
|
||||
void print(const std::string& p = "WhiteNoiseFactor") const {
|
||||
Base::print(p);
|
||||
void print(const std::string& p = "WhiteNoiseFactor",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(p, keyFormatter);
|
||||
std::cout << p + ".z: " << z_ << std::endl;
|
||||
}
|
||||
|
||||
|
|
@ -125,8 +125,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// Calculate the error of the factor, typically equal to log-likelihood
|
||||
inline double error(const VALUES& x) const {
|
||||
return f(z_, x[meanKey_].value(), x[precisionKey_].value());
|
||||
inline double error(const Values& x) const {
|
||||
return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -136,8 +136,8 @@ namespace gtsam {
|
|||
* Here we shoehorn sqrt(2*error(p))
|
||||
* TODO: Where is this used? should disappear.
|
||||
*/
|
||||
virtual Vector unwhitenedError(const VALUES& x) const {
|
||||
return Vector_(1, sqrt(2 * error(x)));
|
||||
virtual Vector unwhitenedError(const Values& x) const {
|
||||
return Vector_(1, std::sqrt(2 * error(x)));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -154,19 +154,20 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x,
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x,
|
||||
const Ordering& ordering) const {
|
||||
double u = x[meanKey_].value();
|
||||
double p = x[precisionKey_].value();
|
||||
double u = x.at<LieScalar>(meanKey_);
|
||||
double p = x.at<LieScalar>(precisionKey_);
|
||||
Index j1 = ordering[meanKey_];
|
||||
Index j2 = ordering[precisionKey_];
|
||||
return linearize(z_, u, p, j1, j2);
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
// TODO: Frank commented this out for now, can it go?
|
||||
// /// @return a deep copy of this factor
|
||||
// virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
// return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
// gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,38 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 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 testWhiteNoiseFactor.cpp
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/WhiteNoiseFactor.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( WhiteNoiseFactor, constructor )
|
||||
{
|
||||
double z = 0.1;
|
||||
Key meanKey=1, precisionKey=2;
|
||||
WhiteNoiseFactor factor(z,meanKey, precisionKey);
|
||||
LONGS_EQUAL(2,factor.dim());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,5 +1,10 @@
|
|||
# Install headers
|
||||
set (slam_excluded_headers #"")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/serialization.h"
|
||||
)
|
||||
|
||||
file(GLOB slam_headers "*.h")
|
||||
list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
|
||||
install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
|
|
@ -15,8 +20,8 @@ set(slam_local_libs
|
|||
|
||||
# Files to exclude from compilation of tests and timing scripts
|
||||
set(slam_excluded_files
|
||||
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
|
||||
"" # Add to this list, with full path, to exclude
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
|
||||
# "" # Add to this list, with full path, to exclude
|
||||
)
|
||||
|
||||
# Build tests
|
||||
|
|
|
|||
|
|
@ -14,11 +14,24 @@ set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRA
|
|||
|
||||
add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
|
||||
|
||||
# To exclude a source from the library build (in any subfolder)
|
||||
# Add the full name to this list, as in the following example
|
||||
# Sources to remove from builds
|
||||
set (excluded_sources # "")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
|
||||
)
|
||||
|
||||
set (excluded_headers # "")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
|
||||
)
|
||||
|
||||
# assemble core libaries
|
||||
foreach(subdir ${gtsam_unstable_subdirs})
|
||||
# Build convenience libraries
|
||||
file(GLOB subdir_srcs "${subdir}/*.cpp")
|
||||
file(GLOB subdir_headers "${subdir}/*.h")
|
||||
list(REMOVE_ITEM subdir_srcs ${excluded_sources})
|
||||
list(REMOVE_ITEM subdir_headers ${excluded_headers})
|
||||
set(${subdir}_srcs ${subdir_srcs} ${subdir_headers})
|
||||
gtsam_assign_source_folders("${${subdir}_srcs}") # Create MSVC structure
|
||||
if (subdir_srcs AND GTSAM_BUILD_CONVENIENCE_LIBRARIES)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,210 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 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 SmartRangeExample_plaza2.cpp
|
||||
* @brief A 2D Range SLAM example
|
||||
* @date June 20, 2013
|
||||
* @author FRank Dellaert
|
||||
*/
|
||||
|
||||
// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
// Each variable in the system (poses and landmarks) must be identified with a unique key.
|
||||
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
// Here we will use Symbols
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
||||
// We want to use iSAM2 to solve the range-SLAM problem incrementally
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
|
||||
// and initial guesses for any new variables used in the added factors
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
// We will use a non-liear solver to batch-inituialize from the first 150 frames
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
|
||||
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
// have been provided with the library for solving robotics SLAM problems.
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/RangeFactor.h>
|
||||
|
||||
// Standard headers, added last, so we know headers above work on their own
|
||||
#include <boost/foreach.hpp>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
namespace NM = gtsam::noiseModel;
|
||||
|
||||
// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
|
||||
// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
|
||||
|
||||
// load the odometry
|
||||
// DR: Odometry Input (delta distance traveled and delta heading change)
|
||||
// Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
|
||||
typedef pair<double, Pose2> TimedOdometry;
|
||||
list<TimedOdometry> readOdometry() {
|
||||
list<TimedOdometry> odometryList;
|
||||
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt");
|
||||
if (!is)
|
||||
throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
|
||||
|
||||
while (is) {
|
||||
double t, distance_traveled, delta_heading;
|
||||
is >> t >> distance_traveled >> delta_heading;
|
||||
odometryList.push_back(
|
||||
TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
return odometryList;
|
||||
}
|
||||
|
||||
// load the ranges from TD
|
||||
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||
typedef boost::tuple<double, size_t, double> RangeTriple;
|
||||
vector<RangeTriple> readTriples() {
|
||||
vector<RangeTriple> triples;
|
||||
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt");
|
||||
if (!is)
|
||||
throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
|
||||
|
||||
while (is) {
|
||||
double t, sender, receiver, range;
|
||||
is >> t >> sender >> receiver >> range;
|
||||
triples.push_back(RangeTriple(t, receiver, range));
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
return triples;
|
||||
}
|
||||
|
||||
// main
|
||||
int main (int argc, char** argv) {
|
||||
|
||||
// load Plaza1 data
|
||||
list<TimedOdometry> odometry = readOdometry();
|
||||
// size_t M = odometry.size();
|
||||
|
||||
vector<RangeTriple> triples = readTriples();
|
||||
size_t K = triples.size();
|
||||
|
||||
// parameters
|
||||
size_t incK = 50; // minimum number of range measurements to process after
|
||||
bool robust = false;
|
||||
|
||||
// Set Noise parameters
|
||||
Vector priorSigmas = Vector3(1,1,M_PI);
|
||||
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
|
||||
double sigmaR = 100; // range standard deviation
|
||||
const NM::Base::shared_ptr // all same type
|
||||
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
|
||||
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
|
||||
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
|
||||
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
|
||||
rangeNoise = robust ? tukey : gaussian;
|
||||
|
||||
// Initialize iSAM
|
||||
ISAM2 isam;
|
||||
|
||||
// Add prior on first pose
|
||||
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
|
||||
M_PI - 2.02108900000000);
|
||||
NonlinearFactorGraph newFactors;
|
||||
newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
|
||||
|
||||
// initialize points (Gaussian)
|
||||
Values initial;
|
||||
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
|
||||
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
|
||||
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
|
||||
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
|
||||
Values landmarkEstimates = initial; // copy landmarks
|
||||
initial.insert(0, pose0);
|
||||
|
||||
// set some loop variables
|
||||
size_t i = 1; // step counter
|
||||
size_t k = 0; // range measurement counter
|
||||
Pose2 lastPose = pose0;
|
||||
size_t countK = 0;
|
||||
|
||||
// Loop over odometry
|
||||
gttic_(iSAM);
|
||||
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
|
||||
//--------------------------------- odometry loop -----------------------------------------
|
||||
double t;
|
||||
Pose2 odometry;
|
||||
boost::tie(t, odometry) = timedOdometry;
|
||||
|
||||
// add odometry factor
|
||||
newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
|
||||
|
||||
// predict pose and add as initial estimate
|
||||
Pose2 predictedPose = lastPose.compose(odometry);
|
||||
lastPose = predictedPose;
|
||||
initial.insert(i, predictedPose);
|
||||
landmarkEstimates.insert(i, predictedPose);
|
||||
|
||||
// Check if there are range factors to be added
|
||||
while (k < K && t >= boost::get<0>(triples[k])) {
|
||||
size_t j = boost::get<1>(triples[k]);
|
||||
double range = boost::get<2>(triples[k]);
|
||||
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,rangeNoise);
|
||||
// Throw out obvious outliers based on current landmark estimates
|
||||
Vector error=factor.unwhitenedError(landmarkEstimates);
|
||||
if (k<=200 || fabs(error[0])<5)
|
||||
newFactors.add(factor);
|
||||
k = k + 1;
|
||||
countK = countK + 1;
|
||||
}
|
||||
|
||||
// Check whether to update iSAM 2
|
||||
if (countK > incK) {
|
||||
gttic_(update);
|
||||
isam.update(newFactors, initial);
|
||||
gttoc_(update);
|
||||
gttic_(calculateEstimate);
|
||||
Values result = isam.calculateEstimate();
|
||||
gttoc_(calculateEstimate);
|
||||
lastPose = result.at<Pose2>(i);
|
||||
// update landmark estimates
|
||||
landmarkEstimates = Values();
|
||||
landmarkEstimates.insert(symbol('L', 1), result.at(symbol('L', 1)));
|
||||
landmarkEstimates.insert(symbol('L', 6), result.at(symbol('L', 6)));
|
||||
landmarkEstimates.insert(symbol('L', 0), result.at(symbol('L', 0)));
|
||||
landmarkEstimates.insert(symbol('L', 5), result.at(symbol('L', 5)));
|
||||
newFactors = NonlinearFactorGraph();
|
||||
initial = Values();
|
||||
countK = 0;
|
||||
}
|
||||
i += 1;
|
||||
//--------------------------------- odometry loop -----------------------------------------
|
||||
} // BOOST_FOREACH
|
||||
gttoc_(iSAM);
|
||||
|
||||
// Print timings
|
||||
tictoc_print_();
|
||||
|
||||
// Write result to file
|
||||
Values result = isam.calculateEstimate();
|
||||
ofstream os2("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
|
||||
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
|
||||
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl;
|
||||
ofstream os("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
|
||||
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
|
||||
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#!/opt/local/bin/gnuplot
|
||||
reset
|
||||
#set terminal pdf
|
||||
set title "Smart range SLAM result"
|
||||
set size ratio 1
|
||||
plot "rangeResult.txt" using 2:3 with lines, "rangeResultLM.txt" using 2:3:4 with circles
|
||||
|
|
@ -86,6 +86,8 @@ virtual class PoseRTV : gtsam::Value {
|
|||
Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
|
||||
gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
|
||||
Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
||||
|
|
@ -122,6 +124,8 @@ virtual class Pose3Upright : gtsam::Value {
|
|||
|
||||
static gtsam::Pose3Upright Expmap(Vector xi);
|
||||
static Vector Logmap(const gtsam::Pose3Upright& p);
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
}; // \class Pose3Upright
|
||||
|
||||
#include <gtsam_unstable/geometry/BearingS2.h>
|
||||
|
|
@ -142,6 +146,8 @@ virtual class BearingS2 : gtsam::Value {
|
|||
size_t dim() const;
|
||||
gtsam::BearingS2 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::BearingS2& p2) const;
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
};
|
||||
|
||||
// std::vector<gtsam::Point2>
|
||||
|
|
@ -288,6 +294,8 @@ class SimPolygon2D {
|
|||
template<T = {gtsam::PoseRTV}>
|
||||
virtual class PriorFactor : gtsam::NonlinearFactor {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -295,6 +303,8 @@ virtual class PriorFactor : gtsam::NonlinearFactor {
|
|||
template<T = {gtsam::PoseRTV}>
|
||||
virtual class BetweenFactor : gtsam::NonlinearFactor {
|
||||
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -302,6 +312,8 @@ virtual class BetweenFactor : gtsam::NonlinearFactor {
|
|||
template<POSE, POINT>
|
||||
virtual class RangeFactor : gtsam::NonlinearFactor {
|
||||
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
};
|
||||
|
||||
typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
|
||||
|
|
@ -314,6 +326,8 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor {
|
|||
NonlinearEquality(size_t j, const T& feasible);
|
||||
// Constructor - allows inexact evaluation
|
||||
NonlinearEquality(size_t j, const T& feasible, double error_gain);
|
||||
|
||||
void serializable() const; // enabling serialization functionality
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/dynamics/IMUFactor.h>
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
|
||||
GTSAM_UNSTABLE_EXPORT friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
|
||||
|
||||
/** Default constructor */
|
||||
ConcurrentSmoother() {};
|
||||
|
|
|
|||
|
|
@ -1,5 +1,10 @@
|
|||
# Install headers
|
||||
set (slam_excluded_headers #"")
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/serialization.h"
|
||||
)
|
||||
|
||||
file(GLOB slam_headers "*.h")
|
||||
list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
|
||||
install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
|
|
@ -19,8 +24,10 @@ set (slam_full_libs
|
|||
${gtsam_unstable-default})
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (slam_excluded_tests "")
|
||||
|
||||
set (slam_excluded_tests
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
|
||||
# "" # Add to this list, with full path, to exclude
|
||||
)
|
||||
# Add all tests
|
||||
gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}")
|
||||
add_dependencies(check.unstable check.slam_unstable)
|
||||
|
|
|
|||
|
|
@ -83,14 +83,21 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u,
|
|||
const double dt) const {
|
||||
// integrate rotation nRb based on gyro measurement u and bias x_g
|
||||
|
||||
#ifndef MODEL_NAV_FRAME_ROTATION
|
||||
// get body to inertial (measured in b) from gyro, subtract bias
|
||||
Vector b_omega_ib = u - x_g_;
|
||||
|
||||
// assume nav to inertial through movement is unknown
|
||||
// nav to inertial due to Earth's rotation and our movement on Earth surface
|
||||
// TODO: figure out how to do this if we need it
|
||||
Vector b_omega_in = zero(3);
|
||||
|
||||
// get angular velocity on bRn measured in body frame
|
||||
// calculate angular velocity on bRn measured in body frame
|
||||
Vector b_omega_bn = b_omega_in - b_omega_ib;
|
||||
#else
|
||||
// Assume a non-rotating nav frame (probably an approximation)
|
||||
// calculate angular velocity on bRn measured in body frame
|
||||
Vector b_omega_bn = x_g_ - u;
|
||||
#endif
|
||||
|
||||
// convert to navigation frame
|
||||
Rot3 nRb = bRn_.inverse();
|
||||
|
|
|
|||
|
|
@ -0,0 +1,130 @@
|
|||
/**
|
||||
* @file SmartRangeFactor.h
|
||||
*
|
||||
* @brief A smart factor for range-only SLAM that does initialization and marginalization
|
||||
*
|
||||
* @date Sep 10, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/base/dllexport.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <map>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Smart factor for range SLAM
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
class GTSAM_UNSTABLE_EXPORT SmartRangeFactor: public NoiseModelFactor {
|
||||
protected:
|
||||
|
||||
struct Circle2 {
|
||||
Circle2(const Point2& p, double r) :
|
||||
center(p), radius(r) {
|
||||
}
|
||||
Point2 center;
|
||||
double radius;
|
||||
};
|
||||
|
||||
/// Range measurements
|
||||
std::vector<double> measurements_;
|
||||
|
||||
public:
|
||||
|
||||
/** Default constructor: don't use directly */
|
||||
SmartRangeFactor() {
|
||||
}
|
||||
|
||||
/** standard binary constructor */
|
||||
SmartRangeFactor(const SharedNoiseModel& model) {
|
||||
}
|
||||
|
||||
virtual ~SmartRangeFactor() {
|
||||
}
|
||||
|
||||
/// Add a range measurement to a pose with given key.
|
||||
void addRange(Key key, double measuredRange) {
|
||||
keys_.push_back(key);
|
||||
measurements_.push_back(measuredRange);
|
||||
}
|
||||
|
||||
// Testable
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
}
|
||||
|
||||
/** Check if two factors are equal */
|
||||
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
// factor interface
|
||||
|
||||
/**
|
||||
* Triangulate a point from at least three pose-range pairs
|
||||
* Checks for best pair that includes first point
|
||||
*/
|
||||
static Point2 triangulate(const std::list<Circle2>& circles) {
|
||||
Circle2 circle1 = circles.front();
|
||||
boost::optional<Point2> best_fh;
|
||||
boost::optional<Circle2> best_circle;
|
||||
BOOST_FOREACH(const Circle2& it, circles) {
|
||||
// distance between circle centers.
|
||||
double d = circle1.center.dist(it.center);
|
||||
if (d < 1e-9)
|
||||
continue;
|
||||
boost::optional<Point2> fh = Point2::CircleCircleIntersection(
|
||||
circle1.radius / d, it.radius / d);
|
||||
if (fh && (!best_fh || fh->y() > best_fh->y())) {
|
||||
best_fh = fh;
|
||||
best_circle = it;
|
||||
}
|
||||
}
|
||||
std::list<Point2> solutions = Point2::CircleCircleIntersection(
|
||||
circle1.center, best_circle->center, best_fh);
|
||||
// TODO, pick winner based on other measurement
|
||||
return solutions.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
|
||||
*/
|
||||
virtual Vector unwhitenedError(const Values& x,
|
||||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||
size_t K = size();
|
||||
Vector errors = zero(K);
|
||||
if (K >= 3) {
|
||||
std::list<Circle2> circles;
|
||||
for (size_t i = 0; i < K; i++) {
|
||||
const Pose2& pose = x.at<Pose2>(keys_[i]);
|
||||
circles.push_back(Circle2(pose.translation(), measurements_[i]));
|
||||
}
|
||||
Point2 optimizedPoint = triangulate(circles);
|
||||
if (H)
|
||||
*H = std::vector<Matrix>();
|
||||
for (size_t i = 0; i < K; i++) {
|
||||
const Pose2& pose = x.at<Pose2>(keys_[i]);
|
||||
if (H) {
|
||||
Matrix Hi;
|
||||
errors[i] = pose.range(optimizedPoint, Hi) - measurements_[i];
|
||||
H->push_back(Hi);
|
||||
} else
|
||||
errors[i] = pose.range(optimizedPoint) - measurements_[i];
|
||||
}
|
||||
}
|
||||
return errors;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -5,11 +5,12 @@
|
|||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#include <list>
|
||||
#include "../AHRS.h"
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include "../AHRS.h"
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <list>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -67,6 +68,21 @@ TEST (AHRS, constructor) {
|
|||
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO make a testMechanization_bRn2
|
||||
TEST (AHRS, Mechanization_integrate) {
|
||||
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
||||
Mechanization_bRn2 mech;
|
||||
KalmanFilter::State state;
|
||||
// boost::tie(mech,state) = ahrs.initialize(g_e);
|
||||
// Vector u = Vector_(3,0.05,0.0,0.0);
|
||||
// double dt = 2;
|
||||
// Rot3 expected;
|
||||
// Mechanization_bRn2 mech2 = mech.integrate(u,dt);
|
||||
// Rot3 actual = mech2.bRn();
|
||||
// EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* TODO: currently fails because of problem with ill-conditioned system
|
||||
TEST (AHRS, init) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,89 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 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 testSmartRangeFactor.cpp
|
||||
* @brief Unit tests for SmartRangeFactor Class
|
||||
* @author Frank Dellaert
|
||||
* @date Nov 2013
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/slam/SmartRangeFactor.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
const noiseModel::Base::shared_ptr gaussian = noiseModel::Isotropic::Sigma(1,
|
||||
2.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST( SmartRangeFactor, constructor ) {
|
||||
SmartRangeFactor f1;
|
||||
LONGS_EQUAL(0, f1.size())
|
||||
SmartRangeFactor f2(gaussian);
|
||||
LONGS_EQUAL(0, f2.size())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST( SmartRangeFactor, addRange ) {
|
||||
SmartRangeFactor f(gaussian);
|
||||
f.addRange(1, 10);
|
||||
f.addRange(1, 12);
|
||||
LONGS_EQUAL(2, f.size())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST( SmartRangeFactor, unwhitenedError ) {
|
||||
// Test situation:
|
||||
Point2 p(0, 10);
|
||||
Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
|
||||
double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(p);
|
||||
DOUBLES_EQUAL(10, r1, 1e-9);
|
||||
DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9);
|
||||
DOUBLES_EQUAL(sqrt(50), r3, 1e-9);
|
||||
|
||||
Values values; // all correct
|
||||
values.insert(1, pose1);
|
||||
values.insert(2, pose2);
|
||||
values.insert(3, pose3);
|
||||
|
||||
SmartRangeFactor f(gaussian);
|
||||
f.addRange(1, r1);
|
||||
|
||||
// Whenever there are two ranges or less, error should be zero
|
||||
Vector actual1 = f.unwhitenedError(values);
|
||||
EXPECT(assert_equal(Vector_(1,0.0), actual1));
|
||||
f.addRange(2, r2);
|
||||
Vector actual2 = f.unwhitenedError(values);
|
||||
EXPECT(assert_equal(Vector2(0,0), actual2));
|
||||
|
||||
f.addRange(3, r3);
|
||||
vector<Matrix> H;
|
||||
Vector actual3 = f.unwhitenedError(values,H);
|
||||
EXPECT(assert_equal(Vector3(0,0,0), actual3));
|
||||
|
||||
// Check keys and Jacobian
|
||||
EXPECT_LONGS_EQUAL(3,f.keys().size());
|
||||
EXPECT(assert_equal(Matrix_(1,3,0.0,-1.0,0.0), H.front()));
|
||||
EXPECT(assert_equal(Matrix_(1,3,sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -37,6 +37,7 @@
|
|||
%
|
||||
%% Linear Inference
|
||||
% GaussianSequentialSolver - class GaussianSequentialSolver, see Doxygen page for details
|
||||
% GaussianMultifrontalSolver - class GaussianMultifrontalSolver, see Doxygen page for details
|
||||
% IterativeOptimizationParameters - class IterativeOptimizationParameters, see Doxygen page for details
|
||||
% KalmanFilter - class KalmanFilter, see Doxygen page for details
|
||||
% SubgraphSolver - class SubgraphSolver, see Doxygen page for details
|
||||
|
|
|
|||
|
|
@ -2,8 +2,9 @@ function [data,truth] = VisualISAMGenerateData(options)
|
|||
% VisualISAMGenerateData creates data for viusalSLAM::iSAM examples
|
||||
% Authors: Duy Nguyen Ta and Frank Dellaert
|
||||
|
||||
%% Generate simulated data
|
||||
import gtsam.*
|
||||
|
||||
%% Generate simulated data
|
||||
if options.triangle % Create a triangle target, just 3 points on a plane
|
||||
nrPoints = 3;
|
||||
r = 10;
|
||||
|
|
@ -24,7 +25,6 @@ else % 3D landmarks as vertices of a cube
|
|||
end
|
||||
|
||||
%% Create camera cameras on a circle around the triangle
|
||||
import gtsam.*
|
||||
height = 10; r = 40;
|
||||
truth.K = Cal3_S2(500,500,0,640/2,480/2);
|
||||
data.K = truth.K;
|
||||
|
|
|
|||
|
|
@ -2,16 +2,16 @@ function [noiseModels,isam,result,nextPoseIndex] = VisualISAMInitialize(data,tru
|
|||
% VisualISAMInitialize initializes visualSLAM::iSAM object and noise parameters
|
||||
% Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
|
||||
|
||||
%% Initialize iSAM
|
||||
import gtsam.*
|
||||
|
||||
%% Initialize iSAM
|
||||
params = gtsam.ISAM2Params;
|
||||
if options.alwaysRelinearize
|
||||
params.setRelinearizeSkip(1);
|
||||
end
|
||||
isam = ISAM2;
|
||||
isam = ISAM2(params);
|
||||
|
||||
%% Set Noise parameters
|
||||
import gtsam.*
|
||||
noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
|
||||
%noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
|
||||
noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]');
|
||||
|
|
@ -20,7 +20,6 @@ noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0);
|
|||
|
||||
%% Add constraints/priors
|
||||
% TODO: should not be from ground truth!
|
||||
import gtsam.*
|
||||
newFactors = NonlinearFactorGraph;
|
||||
initialEstimates = Values;
|
||||
for i=1:2
|
||||
|
|
@ -38,7 +37,6 @@ end
|
|||
nextPoseIndex = 3;
|
||||
|
||||
%% Add visual measurement factors from two first poses and initialize observed landmarks
|
||||
import gtsam.*
|
||||
for i=1:2
|
||||
ii = symbol('x',i);
|
||||
for k=1:length(data.Z{i})
|
||||
|
|
@ -56,11 +54,9 @@ for i=1:2
|
|||
end
|
||||
|
||||
%% Add odometry between frames 1 and 2
|
||||
import gtsam.*
|
||||
newFactors.add(BetweenFactorPose3(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry));
|
||||
|
||||
%% Update ISAM
|
||||
import gtsam.*
|
||||
if options.batchInitialization % Do a full optimize for first two poses
|
||||
batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initialEstimates);
|
||||
fullyOptimized = batchOptimizer.optimize();
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@ function VisualISAMPlot(truth, data, isam, result, options)
|
|||
% VisualISAMPlot plots current state of ISAM2 object
|
||||
% Authors: Duy Nguyen Ta and Frank Dellaert
|
||||
|
||||
import gtsam.*
|
||||
h=gca;
|
||||
cla(h);
|
||||
hold on;
|
||||
|
|
@ -12,7 +13,6 @@ marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO
|
|||
gtsam.plot3DPoints(result, [], marginals);
|
||||
|
||||
%% Plot cameras
|
||||
import gtsam.*
|
||||
M = 1;
|
||||
while result.exists(symbol('x',M))
|
||||
ii = symbol('x',M);
|
||||
|
|
|
|||
|
|
@ -2,19 +2,18 @@ function [isam,result,nextPoseIndex] = VisualISAMStep(data,noiseModels,isam,resu
|
|||
% VisualISAMStep executes one update step of visualSLAM::iSAM object
|
||||
% Authors: Duy Nguyen Ta and Frank Dellaert
|
||||
|
||||
import gtsam.*
|
||||
|
||||
% iSAM expects us to give it a new set of factors
|
||||
% along with initial estimates for any new variables introduced.
|
||||
import gtsam.*
|
||||
newFactors = NonlinearFactorGraph;
|
||||
initialEstimates = Values;
|
||||
|
||||
%% Add odometry
|
||||
import gtsam.*
|
||||
odometry = data.odometry{nextPoseIndex-1};
|
||||
newFactors.add(BetweenFactorPose3(symbol('x',nextPoseIndex-1), symbol('x',nextPoseIndex), odometry, noiseModels.odometry));
|
||||
|
||||
%% Add visual measurement factors and initializations as necessary
|
||||
import gtsam.*
|
||||
for k=1:length(data.Z{nextPoseIndex})
|
||||
zij = data.Z{nextPoseIndex}{k};
|
||||
j = data.J{nextPoseIndex}{k};
|
||||
|
|
@ -28,7 +27,6 @@ for k=1:length(data.Z{nextPoseIndex})
|
|||
end
|
||||
|
||||
%% Initial estimates for the new pose.
|
||||
import gtsam.*
|
||||
prevPose = result.at(symbol('x',nextPoseIndex-1));
|
||||
initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@ install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINA
|
|||
message(STATUS "Installing Matlab Toolbox Examples (Data)")
|
||||
# Data files: *.graph and *.txt
|
||||
file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
|
||||
file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
|
||||
file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
|
||||
set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_txt})
|
||||
set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt})
|
||||
install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,195 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 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
|
||||
%
|
||||
% @brief Read Robotics Institute range-only Plaza2 dataset and do iSAM
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%% preliminaries
|
||||
clear
|
||||
import gtsam.*
|
||||
|
||||
%% Find and load data file
|
||||
% data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
|
||||
% Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
|
||||
% GT: Groundtruth path from GPS
|
||||
% Time (sec) X_pose (m) Y_pose (m) Heading (rad)
|
||||
% DR: Odometry Input (delta distance traveled and delta heading change)
|
||||
% Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
|
||||
% DRp: Dead Reckoned Path from Odometry
|
||||
% Time (sec) X_pose (m) Y_pose (m) Heading (rad)
|
||||
% TL: Surveyed Node Locations
|
||||
% Time (sec) X_pose (m) Y_pose (m)
|
||||
% TD
|
||||
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||
if false % switch between data files
|
||||
datafile = findExampleDataFile('Plaza1_.mat');
|
||||
headingOffset=0;
|
||||
minK=200; % minimum number of range measurements to process initially
|
||||
incK=5; % minimum number of range measurements to process after
|
||||
else
|
||||
datafile = findExampleDataFile('Plaza2_.mat');
|
||||
headingOffset=pi;
|
||||
minK=150; % needs less for init
|
||||
incK=25; % minimum number of range measurements to process after
|
||||
end
|
||||
load(datafile)
|
||||
M=size(DR,1);
|
||||
K=size(TD,1);
|
||||
sigmaR = 100; % range standard deviation
|
||||
sigmaInitial = 1; % draw initial landmark guess from Gaussian
|
||||
useGroundTruth = false;
|
||||
useRobust=true;
|
||||
addRange=true;
|
||||
batchInitialization=true;
|
||||
|
||||
%% Set Noise parameters
|
||||
noiseModels.prior = noiseModel.Diagonal.Sigmas([1 1 pi]');
|
||||
noiseModels.pointPrior = noiseModel.Diagonal.Sigmas([1 1]');
|
||||
noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.01 0.2]');
|
||||
if useRobust
|
||||
base = noiseModel.mEstimator.Tukey(15);
|
||||
noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
|
||||
else
|
||||
noiseModels.range = noiseModel.Isotropic.Sigma(1, sigmaR);
|
||||
end
|
||||
|
||||
%% Initialize iSAM
|
||||
isam = ISAM2;
|
||||
|
||||
%% Add prior on first pose
|
||||
pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4));
|
||||
newFactors = NonlinearFactorGraph;
|
||||
if ~addRange | ~useGroundTruth
|
||||
newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior));
|
||||
end
|
||||
initial = Values;
|
||||
initial.insert(0,pose0);
|
||||
odo = Values;
|
||||
odo.insert(0,pose0);
|
||||
|
||||
%% initialize points
|
||||
if addRange
|
||||
landmarkEstimates = Values;
|
||||
for i=1:size(TL,1)
|
||||
j=TL(i,1);
|
||||
if useGroundTruth
|
||||
Lj = Point2(TL(i,2),TL(i,3));
|
||||
newFactors.add(PriorFactorPoint2(symbol('L',j),Lj,noiseModels.pointPrior));
|
||||
else
|
||||
Lj = Point2(sigmaInitial*randn,sigmaInitial*randn);
|
||||
end
|
||||
initial.insert(symbol('L',j),Lj);
|
||||
landmarkEstimates.insert(symbol('L',j),Lj);
|
||||
end
|
||||
XY = utilities.extractPoint2(initial);
|
||||
plot(XY(:,1),XY(:,2),'g*');
|
||||
end
|
||||
|
||||
%% Loop over odometry
|
||||
tic
|
||||
k = 1; % range measurement counter
|
||||
update = false;
|
||||
lastPose = pose0;
|
||||
odoPose = pose0;
|
||||
countK = 0;
|
||||
for i=1:M % M
|
||||
|
||||
% get odometry measurement
|
||||
t = DR(i,1);
|
||||
distance_traveled = DR(i,2);
|
||||
delta_heading = DR(i,3);
|
||||
|
||||
% add odometry factor
|
||||
odometry = Pose2(distance_traveled,0,delta_heading);
|
||||
newFactors.add(BetweenFactorPose2(i-1, i, odometry, noiseModels.odometry));
|
||||
|
||||
% predict pose and update odometry
|
||||
predictedOdo = odoPose.compose(odometry);
|
||||
odoPose = predictedOdo;
|
||||
odo.insert(i,predictedOdo);
|
||||
|
||||
% predict pose and add as initial estimate
|
||||
predictedPose = lastPose.compose(odometry);
|
||||
lastPose = predictedPose;
|
||||
initial.insert(i,predictedPose);
|
||||
landmarkEstimates.insert(i,predictedPose);
|
||||
|
||||
% Check if there are range factors to be added
|
||||
while k<=K && t>=TD(k,1)
|
||||
j = TD(k,3);
|
||||
range = TD(k,4);
|
||||
if addRange
|
||||
factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
|
||||
% Throw out obvious outliers based on current landmark estimates
|
||||
error=factor.unwhitenedError(landmarkEstimates);
|
||||
if k<=minK || abs(error)<5
|
||||
newFactors.add(factor);
|
||||
end
|
||||
end
|
||||
k=k+1; countK=countK+1; update = true;
|
||||
end
|
||||
|
||||
% Check whether to update iSAM 2
|
||||
if update && k>minK && countK>incK
|
||||
if batchInitialization % Do a full optimize for first minK ranges
|
||||
tic
|
||||
batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initial);
|
||||
initial = batchOptimizer.optimize();
|
||||
toc
|
||||
batchInitialization = false; % only once
|
||||
end
|
||||
isam.update(newFactors, initial);
|
||||
result = isam.calculateEstimate();
|
||||
lastPose = result.at(i);
|
||||
% update landmark estimates
|
||||
if addRange
|
||||
landmarkEstimates = Values;
|
||||
for jj=1:size(TL,1)
|
||||
j=TL(jj,1);
|
||||
key = symbol('L',j);
|
||||
landmarkEstimates.insert(key,result.at(key));
|
||||
end
|
||||
end
|
||||
newFactors = NonlinearFactorGraph;
|
||||
initial = Values;
|
||||
countK = 0;
|
||||
end
|
||||
|
||||
% visualize
|
||||
if mod(i,50)==0 && k>minK
|
||||
figure(1);clf;hold on
|
||||
|
||||
% odometry
|
||||
XYT = utilities.extractPose2(odo);
|
||||
plot(XYT(:,1),XYT(:,2),'y-');
|
||||
|
||||
% lin point
|
||||
lin = isam.getLinearizationPoint();
|
||||
XYT = utilities.extractPose2(lin);
|
||||
plot(XYT(:,1),XYT(:,2),'r.');
|
||||
XY = utilities.extractPoint2(lin);
|
||||
plot(XY(:,1),XY(:,2),'r*');
|
||||
|
||||
% result
|
||||
result = isam.calculateEstimate();
|
||||
XYT = utilities.extractPose2(result);
|
||||
plot(XYT(:,1),XYT(:,2),'k-');
|
||||
XY = utilities.extractPoint2(result);
|
||||
plot(XY(:,1),XY(:,2),'k*');
|
||||
axis equal
|
||||
% pause
|
||||
end
|
||||
end
|
||||
toc
|
||||
|
||||
%% Plot ground truth as well
|
||||
plot(GT(:,2),GT(:,3),'g-');
|
||||
plot(TL(:,2),TL(:,3),'g*');
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 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
|
||||
%
|
||||
% @brief Read Robotics Institute range-only Plaza2 dataset and do SAM
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%% preliminaries
|
||||
clear
|
||||
import gtsam.*
|
||||
|
||||
%% Find and load data file
|
||||
% data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
|
||||
% Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
|
||||
% GT: Groundtruth path from GPS
|
||||
% Time (sec) X_pose (m) Y_pose (m) Heading (rad)
|
||||
% DR: Odometry Input (delta distance traveled and delta heading change)
|
||||
% Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
|
||||
% DRp: Dead Reckoned Path from Odometry
|
||||
% Time (sec) X_pose (m) Y_pose (m) Heading (rad)
|
||||
% TL: Surveyed Node Locations
|
||||
% Time (sec) X_pose (m) Y_pose (m)
|
||||
% TD
|
||||
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
|
||||
datafile = findExampleDataFile('Plaza1_.mat');
|
||||
load(datafile)
|
||||
M=size(DR,1);
|
||||
K=size(TD,1);
|
||||
sigmaR = 50; % range standard deviation
|
||||
sigmaInitial = 1; % draw initial landmark guess from Gaussian
|
||||
|
||||
%% Set Noise parameters
|
||||
noiseModels.prior = noiseModel.Diagonal.Sigmas([1 1 pi]');
|
||||
noiseModels.pointPrior = noiseModel.Diagonal.Sigmas([1 1]');
|
||||
noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.01 0.2]');
|
||||
base = noiseModel.mEstimator.Tukey(5);
|
||||
noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
|
||||
|
||||
%% Add prior on first pose
|
||||
pose0 = Pose2(GT(1,2),GT(1,3),GT(1,4));
|
||||
graph = NonlinearFactorGraph;
|
||||
graph.add(PriorFactorPose2(0,pose0,noiseModels.prior));
|
||||
initial = Values;
|
||||
initial.insert(0,pose0);
|
||||
|
||||
for i=1:size(TL,1)
|
||||
j=TL(i,1);
|
||||
initial.insert(symbol('L',j),Point2(sigmaInitial*randn,sigmaInitial*randn));
|
||||
end
|
||||
|
||||
%% Loop over odometry
|
||||
tic
|
||||
k = 1; % range measurement counter
|
||||
lastPose = pose0;
|
||||
for i=1:M
|
||||
|
||||
% get odometry measurement
|
||||
t = DR(i,1);
|
||||
distance_traveled = DR(i,2);
|
||||
delta_heading = DR(i,3);
|
||||
|
||||
% add odometry factor
|
||||
odometry = Pose2(distance_traveled,0,delta_heading);
|
||||
graph.add(BetweenFactorPose2(i-1, i, odometry, noiseModels.odometry));
|
||||
|
||||
% predict pose and add as initial estimate
|
||||
predictedPose = lastPose.compose(odometry);
|
||||
lastPose = predictedPose;
|
||||
initial.insert(i,predictedPose);
|
||||
|
||||
while k<=K && t>=TD(k,1)
|
||||
j = TD(k,3);
|
||||
range = TD(k,4);
|
||||
factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
|
||||
graph.add(factor);
|
||||
k=k+1;
|
||||
end
|
||||
|
||||
end
|
||||
toc
|
||||
|
||||
%% GRaph was built, optimize !
|
||||
tic
|
||||
batchOptimizer = LevenbergMarquardtOptimizer(graph, initial);
|
||||
result = batchOptimizer.optimize();
|
||||
toc
|
||||
|
||||
%% visualize
|
||||
figure(1);clf;hold on
|
||||
|
||||
% odometry
|
||||
XYT = utilities.extractPose2(initial);
|
||||
plot(XYT(:,1),XYT(:,2),'y-');
|
||||
|
||||
% GT
|
||||
plot(GT(:,2),GT(:,3),'g-');
|
||||
plot(TL(:,2),TL(:,3),'g*');
|
||||
|
||||
% result
|
||||
XYT = utilities.extractPose2(result);
|
||||
plot(XYT(:,1),XYT(:,2),'k-');
|
||||
XY = utilities.extractPoint2(result);
|
||||
plot(XY(:,1),XY(:,2),'k*');
|
||||
axis equal
|
||||
|
|
@ -1,65 +0,0 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 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
|
||||
%
|
||||
% @brief Simple robotics example using the pre-built planar SLAM domain
|
||||
% @author Alex Cunningham
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
import gtsam.*
|
||||
|
||||
%% Create keys for variables
|
||||
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
|
||||
j1 = symbol('l',1); j2 = symbol('l',2);
|
||||
|
||||
%% Create graph and factors
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
% Prior factor - FAIL: unregistered class
|
||||
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
|
||||
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
|
||||
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
|
||||
|
||||
% Between Factors - FAIL: unregistered class
|
||||
odometry = Pose2(2.0, 0.0, 0.0);
|
||||
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
|
||||
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
|
||||
|
||||
% BearingRange Factors - FAIL: unregistered class
|
||||
degrees = pi/180;
|
||||
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
|
||||
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
|
||||
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
|
||||
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
|
||||
|
||||
%% Create Values
|
||||
values = Values;
|
||||
values.insert(i1, Pose2(0.5, 0.0, 0.2));
|
||||
values.insert(i2, Pose2(2.3, 0.1,-0.2));
|
||||
values.insert(i3, Pose2(4.1, 0.1, 0.1));
|
||||
values.insert(j1, Point2(1.8, 2.1));
|
||||
values.insert(j2, Point2(4.1, 1.8));
|
||||
|
||||
%% Check that serialization works properly
|
||||
serialized_values = serializeValues(values); % returns a string
|
||||
deserializedValues = deserializeValues(serialized_values); % returns a new values
|
||||
CHECK('values.equals(deserializedValues)',values.equals(deserializedValues,1e-9));
|
||||
|
||||
CHECK('serializeValuesToFile(values, values.dat)', serializeValuesToFile(values, 'values.dat'));
|
||||
deserializedValuesFile = deserializeValuesFromFile('values.dat'); % returns a new values
|
||||
CHECK('values.equals(deserializedValuesFile)',values.equals(deserializedValuesFile,1e-9));
|
||||
|
||||
% % FAIL: unregistered class - derived class not registered or exported
|
||||
% serialized_graph = serializeGraph(graph); % returns a string
|
||||
% deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph
|
||||
% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9));
|
||||
%
|
||||
% CHECK('serializeGraphToFile(graph, graph.dat)', serializeGraphToFile(graph, 'graph.dat'));
|
||||
% deserializedGraphFile = deserializeGraphFromFile('graph.dat'); % returns a new graph
|
||||
% CHECK('graph.equals(deserializedGraphFile)',graph.equals(deserializedGraphFile,1e-9));
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 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
|
||||
%
|
||||
% @brief Checks for serialization using basic string interface
|
||||
% @author Alex Cunningham
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
import gtsam.*
|
||||
|
||||
%% Create keys for variables
|
||||
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
|
||||
j1 = symbol('l',1); j2 = symbol('l',2);
|
||||
|
||||
%% Create values and verify string serialization
|
||||
pose1=Pose2(0.5, 0.0, 0.2);
|
||||
pose2=Pose2(2.3, 0.1,-0.2);
|
||||
pose3=Pose2(4.1, 0.1, 0.1);
|
||||
landmark1=Point2(1.8, 2.1);
|
||||
landmark2=Point2(4.1, 1.8);
|
||||
|
||||
serialized_pose1 = pose1.string_serialize();
|
||||
pose1ds = Pose2.string_deserialize(serialized_pose1);
|
||||
CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9));
|
||||
|
||||
serialized_landmark1 = landmark1.string_serialize();
|
||||
landmark1ds = Point2.string_deserialize(serialized_landmark1);
|
||||
CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9));
|
||||
|
||||
%% Create and serialize Values
|
||||
values = Values;
|
||||
values.insert(i1, pose1);
|
||||
values.insert(i2, pose2);
|
||||
values.insert(i3, pose3);
|
||||
values.insert(j1, landmark1);
|
||||
values.insert(j2, landmark2);
|
||||
|
||||
serialized_values = values.string_serialize();
|
||||
valuesds = Values.string_deserialize(serialized_values);
|
||||
CHECK('valuesds.equals(values, 1e-9)', valuesds.equals(values, 1e-9));
|
||||
|
||||
%% Create graph and factors and serialize
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
% Prior factor
|
||||
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
|
||||
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
|
||||
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
|
||||
|
||||
% Between Factors - FAIL: unregistered class
|
||||
odometry = Pose2(2.0, 0.0, 0.0);
|
||||
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
|
||||
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
|
||||
|
||||
% BearingRange Factors - FAIL: unregistered class
|
||||
degrees = pi/180;
|
||||
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
|
||||
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
|
||||
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
|
||||
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
|
||||
|
||||
serialized_graph = graph.string_serialize();
|
||||
graphds = NonlinearFactorGraph.string_deserialize(serialized_graph);
|
||||
CHECK('graphds.equals(graph, 1e-9)', graphds.equals(graph, 1e-9));
|
||||
|
|
@ -30,8 +30,8 @@ testStereoVOExample
|
|||
display 'Starting: testVisualISAMExample'
|
||||
testVisualISAMExample
|
||||
|
||||
display 'Starting: testGraphValuesSerialization'
|
||||
testVisualISAMExample
|
||||
display 'Starting: testSerialization'
|
||||
testSerialization
|
||||
|
||||
% end of tests
|
||||
display 'Tests complete!'
|
||||
|
|
|
|||
|
|
@ -22,10 +22,6 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Rich
|
|||
list (APPEND tests_exclude "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp")
|
||||
endif()
|
||||
|
||||
if(MSVC)
|
||||
add_definitions("/bigobj") # testSerializationSLAM needs this
|
||||
endif()
|
||||
|
||||
# Build tests
|
||||
if (GTSAM_BUILD_TESTS)
|
||||
# Subdirectory target for tests
|
||||
|
|
@ -34,7 +30,7 @@ if (GTSAM_BUILD_TESTS)
|
|||
|
||||
# Build grouped tests
|
||||
gtsam_add_grouped_scripts("tests" # Use subdirectory as group label
|
||||
"test*.cpp" check "Test" # Standard for all tests
|
||||
"test*.cpp;*.h" check "Test" # Standard for all tests
|
||||
"${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists
|
||||
${is_test}) # Set all as tests
|
||||
endif (GTSAM_BUILD_TESTS)
|
||||
|
|
@ -51,3 +47,8 @@ if (GTSAM_BUILD_TIMING)
|
|||
"${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists
|
||||
${is_test})
|
||||
endif (GTSAM_BUILD_TIMING)
|
||||
|
||||
if(MSVC)
|
||||
set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp"
|
||||
APPEND PROPERTY COMPILE_FLAGS "/bigobj")
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -83,42 +83,43 @@ namespace simulated2D {
|
|||
}
|
||||
};
|
||||
|
||||
namespace {
|
||||
/// Prior on a single pose
|
||||
inline Point2 prior(const Point2& x) {
|
||||
return x;
|
||||
}
|
||||
|
||||
/// Prior on a single pose
|
||||
inline Point2 prior(const Point2& x) {
|
||||
return x;
|
||||
}
|
||||
/// Prior on a single pose, optionally returns derivative
|
||||
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
|
||||
if (H) *H = gtsam::eye(2);
|
||||
return x;
|
||||
}
|
||||
|
||||
/// Prior on a single pose, optionally returns derivative
|
||||
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
|
||||
if (H) *H = gtsam::eye(2);
|
||||
return x;
|
||||
}
|
||||
/// odometry between two poses
|
||||
inline Point2 odo(const Point2& x1, const Point2& x2) {
|
||||
return x2 - x1;
|
||||
}
|
||||
|
||||
/// odometry between two poses
|
||||
inline Point2 odo(const Point2& x1, const Point2& x2) {
|
||||
return x2 - x1;
|
||||
}
|
||||
|
||||
/// odometry between two poses, optionally returns derivative
|
||||
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
|
||||
/// odometry between two poses, optionally returns derivative
|
||||
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) {
|
||||
if (H1) *H1 = -gtsam::eye(2);
|
||||
if (H2) *H2 = gtsam::eye(2);
|
||||
return x2 - x1;
|
||||
}
|
||||
if (H1) *H1 = -gtsam::eye(2);
|
||||
if (H2) *H2 = gtsam::eye(2);
|
||||
return x2 - x1;
|
||||
}
|
||||
|
||||
/// measurement between landmark and pose
|
||||
inline Point2 mea(const Point2& x, const Point2& l) {
|
||||
return l - x;
|
||||
}
|
||||
/// measurement between landmark and pose
|
||||
inline Point2 mea(const Point2& x, const Point2& l) {
|
||||
return l - x;
|
||||
}
|
||||
|
||||
/// measurement between landmark and pose, optionally returns derivative
|
||||
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
|
||||
/// measurement between landmark and pose, optionally returns derivative
|
||||
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) {
|
||||
if (H1) *H1 = -gtsam::eye(2);
|
||||
if (H2) *H2 = gtsam::eye(2);
|
||||
return l - x;
|
||||
if (H1) *H1 = -gtsam::eye(2);
|
||||
if (H2) *H2 = gtsam::eye(2);
|
||||
return l - x;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
namespace example {
|
||||
namespace {
|
||||
|
||||
typedef NonlinearFactorGraph Graph;
|
||||
|
||||
|
|
@ -150,13 +151,9 @@ Ordering planarOrdering(size_t N);
|
|||
std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
|
||||
size_t N, const GaussianFactorGraph& original);
|
||||
|
||||
} // example
|
||||
} // gtsam
|
||||
|
||||
|
||||
// Implementations
|
||||
namespace gtsam {
|
||||
namespace example {
|
||||
|
||||
// using namespace gtsam::noiseModel;
|
||||
|
||||
|
|
@ -649,5 +646,6 @@ std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // anonymous namespace
|
||||
} // \namespace example
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
172
wrap/Class.cpp
172
wrap/Class.cpp
|
|
@ -107,10 +107,12 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
|
|||
// Methods
|
||||
BOOST_FOREACH(const Methods::value_type& name_m, methods) {
|
||||
const Method& m = name_m.second;
|
||||
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames);
|
||||
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames);
|
||||
proxyFile.oss << "\n";
|
||||
wrapperFile.oss << "\n";
|
||||
}
|
||||
if (hasSerialization)
|
||||
serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
|
||||
|
||||
proxyFile.oss << " end\n";
|
||||
proxyFile.oss << "\n";
|
||||
|
|
@ -123,7 +125,9 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
|
|||
proxyFile.oss << "\n";
|
||||
wrapperFile.oss << "\n";
|
||||
}
|
||||
|
||||
if (hasSerialization)
|
||||
deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
|
||||
|
||||
proxyFile.oss << " end\n";
|
||||
proxyFile.oss << "end\n";
|
||||
|
||||
|
|
@ -278,6 +282,7 @@ Class expandClassTemplate(const Class& cls, const string& templateArg, const vec
|
|||
inst.templateArgs = cls.templateArgs;
|
||||
inst.typedefName = cls.typedefName;
|
||||
inst.isVirtual = cls.isVirtual;
|
||||
inst.isSerializable = cls.isSerializable;
|
||||
inst.qualifiedParent = cls.qualifiedParent;
|
||||
inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName);
|
||||
inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName);
|
||||
|
|
@ -388,6 +393,169 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
|
|||
}
|
||||
}
|
||||
|
||||
if (hasSerialization) {
|
||||
proxyFile.oss << "%\n%-------Serialization Interface-------\n";
|
||||
proxyFile.oss << "%string_serialize() : returns string\n";
|
||||
proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n";
|
||||
}
|
||||
|
||||
proxyFile.oss << "%\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
|
||||
const std::string& wrapperName, std::vector<std::string>& functionNames) const {
|
||||
|
||||
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
//{
|
||||
// typedef boost::shared_ptr<Point3> Shared;
|
||||
// checkArguments("string_serialize",nargout,nargin-1,0);
|
||||
// Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
|
||||
// std::ostringstream out_archive_stream;
|
||||
// boost::archive::text_oarchive out_archive(out_archive_stream);
|
||||
// out_archive << *obj;
|
||||
// out[0] = wrap< string >(out_archive_stream.str());
|
||||
//}
|
||||
|
||||
int serialize_id = functionNames.size();
|
||||
const string
|
||||
matlabQualName = qualifiedName("."),
|
||||
matlabUniqueName = qualifiedName(),
|
||||
cppClassName = qualifiedName("::");
|
||||
const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
|
||||
functionNames.push_back(wrapFunctionNameSerialize);
|
||||
|
||||
// call
|
||||
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
|
||||
wrapperFile.oss << "{\n";
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl;
|
||||
|
||||
// check arguments - for serialize, no arguments
|
||||
// example: checkArguments("string_serialize",nargout,nargin-1,0);
|
||||
wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n";
|
||||
|
||||
// get class pointer
|
||||
// example: Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
|
||||
wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
|
||||
|
||||
// Serialization boilerplate
|
||||
wrapperFile.oss << " std::ostringstream out_archive_stream;\n";
|
||||
wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n";
|
||||
wrapperFile.oss << " out_archive << *obj;\n";
|
||||
wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n";
|
||||
|
||||
// finish
|
||||
wrapperFile.oss << "}\n";
|
||||
|
||||
// Generate code for matlab function
|
||||
// function varargout string_serialize(this, varargin)
|
||||
// % STRING_SERIALIZE usage: string_serialize() : returns string
|
||||
// % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
// if length(varargin) == 0
|
||||
// varargout{1} = geometry_wrapper(15, this, varargin{:});
|
||||
// else
|
||||
// error('Arguments do not match any overload of function Point3.string_serialize');
|
||||
// end
|
||||
// end
|
||||
|
||||
proxyFile.oss << " function varargout = string_serialize(this, varargin)\n";
|
||||
proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n";
|
||||
proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
|
||||
proxyFile.oss << " if length(varargin) == 0\n";
|
||||
proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast<string>(serialize_id) << ", this, varargin{:});\n";
|
||||
proxyFile.oss << " else\n";
|
||||
proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n";
|
||||
proxyFile.oss << " end\n";
|
||||
proxyFile.oss << " end\n\n";
|
||||
|
||||
// Generate code for matlab save function
|
||||
// function sobj = saveobj(obj)
|
||||
// % SAVEOBJ Saves the object to a matlab-readable format
|
||||
// sobj = obj.string_serialize();
|
||||
// end
|
||||
|
||||
proxyFile.oss << " function sobj = saveobj(obj)\n";
|
||||
proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n";
|
||||
proxyFile.oss << " sobj = obj.string_serialize();\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
|
||||
const std::string& wrapperName, std::vector<std::string>& functionNames) const {
|
||||
//void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
//{
|
||||
// typedef boost::shared_ptr<Point3> Shared;
|
||||
// checkArguments("Point3.string_deserialize",nargout,nargin,1);
|
||||
// string serialized = unwrap< string >(in[0]);
|
||||
// std::istringstream in_archive_stream(serialized);
|
||||
// boost::archive::text_iarchive in_archive(in_archive_stream);
|
||||
// Shared output(new Point3());
|
||||
// in_archive >> *output;
|
||||
// out[0] = wrap_shared_ptr(output,"Point3", false);
|
||||
//}
|
||||
int deserialize_id = functionNames.size();
|
||||
const string
|
||||
matlabQualName = qualifiedName("."),
|
||||
matlabUniqueName = qualifiedName(),
|
||||
cppClassName = qualifiedName("::");
|
||||
const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
|
||||
functionNames.push_back(wrapFunctionNameDeserialize);
|
||||
|
||||
// call
|
||||
wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
|
||||
wrapperFile.oss << "{\n";
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl;
|
||||
|
||||
// check arguments - for deserialize, 1 string argument
|
||||
wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n";
|
||||
|
||||
// string argument with deserialization boilerplate
|
||||
wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n";
|
||||
wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n";
|
||||
wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n";
|
||||
wrapperFile.oss << " Shared output(new " << cppClassName << "());\n";
|
||||
wrapperFile.oss << " in_archive >> *output;\n";
|
||||
wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n";
|
||||
wrapperFile.oss << "}\n";
|
||||
|
||||
// Generate matlab function
|
||||
// function varargout = string_deserialize(varargin)
|
||||
// % STRING_DESERIALIZE usage: string_deserialize() : returns Point3
|
||||
// % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
// if length(varargin) == 1
|
||||
// varargout{1} = geometry_wrapper(18, varargin{:});
|
||||
// else
|
||||
// error('Arguments do not match any overload of function Point3.string_deserialize');
|
||||
// end
|
||||
// end
|
||||
|
||||
proxyFile.oss << " function varargout = string_deserialize(varargin)\n";
|
||||
proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n";
|
||||
proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
|
||||
proxyFile.oss << " if length(varargin) == 1\n";
|
||||
proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast<string>(deserialize_id) << ", varargin{:});\n";
|
||||
proxyFile.oss << " else\n";
|
||||
proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n";
|
||||
proxyFile.oss << " end\n";
|
||||
proxyFile.oss << " end\n\n";
|
||||
|
||||
|
||||
// Generate matlab load function
|
||||
// function obj = loadobj(sobj)
|
||||
// % LOADOBJ Saves the object to a matlab-readable format
|
||||
// obj = Point3.string_deserialize(sobj);
|
||||
// end
|
||||
|
||||
proxyFile.oss << " function obj = loadobj(sobj)\n";
|
||||
proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n";
|
||||
proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n";
|
||||
proxyFile.oss << " end" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string Class::getSerializationExport() const {
|
||||
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
|
||||
return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");";
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
16
wrap/Class.h
16
wrap/Class.h
|
|
@ -37,13 +37,15 @@ struct Class {
|
|||
typedef std::map<std::string, StaticMethod> StaticMethods;
|
||||
|
||||
/// Constructor creates an empty class
|
||||
Class(bool verbose=true) : isVirtual(false), verbose_(verbose) {}
|
||||
Class(bool verbose=true) : isVirtual(false), isSerializable(false), hasSerialization(false), verbose_(verbose) {}
|
||||
|
||||
// Then the instance variables are set directly by the Module constructor
|
||||
std::string name; ///< Class name
|
||||
std::vector<std::string> templateArgs; ///< Template arguments
|
||||
std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name]
|
||||
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
|
||||
bool hasSerialization; ///< Whether we should create the serialization functions
|
||||
std::vector<std::string> qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack
|
||||
Methods methods; ///< Class methods
|
||||
StaticMethods static_methods; ///< Static methods
|
||||
|
|
@ -55,14 +57,26 @@ struct Class {
|
|||
// And finally MATLAB code is emitted, methods below called by Module::matlab_code
|
||||
void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
|
||||
FileWriter& wrapperFile, std::vector<std::string>& functionNames) const; ///< emit proxy class
|
||||
|
||||
std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter
|
||||
|
||||
std::vector<Class> expandTemplate(const std::string& templateArg, const std::vector<std::vector<std::string> >& instantiations) const;
|
||||
|
||||
Class expandTemplate(const std::string& templateArg, const std::vector<std::string>& instantiation, const std::vector<std::string>& expandedClassNamespace, const std::string& expandedClassName) const;
|
||||
|
||||
// The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
|
||||
std::string getTypedef() const;
|
||||
|
||||
// Returns the string for an export flag
|
||||
std::string getSerializationExport() const;
|
||||
|
||||
// Creates a member function that performs serialization
|
||||
void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
|
||||
const std::string& wrapperName, std::vector<std::string>& functionNames) const;
|
||||
|
||||
// Creates a static member function that performs deserialization
|
||||
void deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
|
||||
const std::string& wrapperName, std::vector<std::string>& functionNames) const;
|
||||
|
||||
private:
|
||||
void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector<std::string>& functionNames) const;
|
||||
|
|
|
|||
|
|
@ -378,13 +378,28 @@ void Module::parseMarkup(const std::string& data) {
|
|||
throw ParseFailed((int)info.length);
|
||||
}
|
||||
|
||||
//Explicitly add methods to the classes from parents so it shows in documentation
|
||||
// Post-process classes for serialization markers
|
||||
BOOST_FOREACH(Class& cls, classes) {
|
||||
Class::Methods::iterator serializable_it = cls.methods.find("serializable");
|
||||
if (serializable_it != cls.methods.end()) {
|
||||
cls.isSerializable = true;
|
||||
cls.methods.erase(serializable_it);
|
||||
}
|
||||
|
||||
Class::Methods::iterator serialize_it = cls.methods.find("serialize");
|
||||
if (serialize_it != cls.methods.end()) {
|
||||
cls.isSerializable = true;
|
||||
cls.hasSerialization= true;
|
||||
cls.methods.erase(serialize_it);
|
||||
}
|
||||
}
|
||||
|
||||
// Explicitly add methods to the classes from parents so it shows in documentation
|
||||
BOOST_FOREACH(Class& cls, classes)
|
||||
{
|
||||
map<string, Method> inhereted = appendInheretedMethods(cls, classes);
|
||||
cls.methods.insert(inhereted.begin(), inhereted.end());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -440,16 +455,6 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
|
|||
|
||||
fs::create_directories(toolboxPath);
|
||||
|
||||
// create the unified .cpp switch file
|
||||
const string wrapperName = name + "_wrapper";
|
||||
string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
|
||||
FileWriter wrapperFile(wrapperFileName, verbose, "//");
|
||||
vector<string> functionNames; // Function names stored by index for switch
|
||||
wrapperFile.oss << "#include <wrap/matlab.h>\n";
|
||||
wrapperFile.oss << "#include <map>\n";
|
||||
wrapperFile.oss << "#include <boost/foreach.hpp>\n";
|
||||
wrapperFile.oss << "\n";
|
||||
|
||||
// Expand templates - This is done first so that template instantiations are
|
||||
// counted in the list of valid types, have their attributes and dependencies
|
||||
// checked, etc.
|
||||
|
|
@ -462,7 +467,9 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
|
|||
verifyArguments<GlobalFunction>(validTypes, global_functions);
|
||||
verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
|
||||
|
||||
bool hasSerialiable = false;
|
||||
BOOST_FOREACH(const Class& cls, expandedClasses) {
|
||||
hasSerialiable |= cls.isSerializable;
|
||||
// verify all of the function arguments
|
||||
//TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
|
||||
verifyArguments<StaticMethod>(validTypes, cls.static_methods);
|
||||
|
|
@ -475,7 +482,6 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
|
|||
// verify parents
|
||||
if(!cls.qualifiedParent.empty() && std::find(validTypes.begin(), validTypes.end(), wrap::qualifiedName("::", cls.qualifiedParent)) == validTypes.end())
|
||||
throw DependencyMissing(wrap::qualifiedName("::", cls.qualifiedParent), cls.qualifiedName("::"));
|
||||
|
||||
}
|
||||
|
||||
// Create type attributes table and check validity
|
||||
|
|
@ -484,6 +490,22 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
|
|||
typeAttributes.addForwardDeclarations(forward_declarations);
|
||||
typeAttributes.checkValidity(expandedClasses);
|
||||
|
||||
// create the unified .cpp switch file
|
||||
const string wrapperName = name + "_wrapper";
|
||||
string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
|
||||
FileWriter wrapperFile(wrapperFileName, verbose, "//");
|
||||
wrapperFile.oss << "#include <wrap/matlab.h>\n";
|
||||
wrapperFile.oss << "#include <map>\n";
|
||||
wrapperFile.oss << "#include <boost/foreach.hpp>\n";
|
||||
wrapperFile.oss << "\n";
|
||||
|
||||
// Include boost.serialization archive headers before other class headers
|
||||
if (hasSerialiable) {
|
||||
wrapperFile.oss << "#include <boost/serialization/export.hpp>\n";
|
||||
wrapperFile.oss << "#include <boost/archive/text_iarchive.hpp>\n";
|
||||
wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n\n";
|
||||
}
|
||||
|
||||
// Generate includes while avoiding redundant includes
|
||||
generateIncludes(wrapperFile);
|
||||
|
||||
|
|
@ -494,12 +516,23 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
|
|||
}
|
||||
wrapperFile.oss << "\n";
|
||||
|
||||
// Generate boost.serialization export flags (needs typedefs from above)
|
||||
if (hasSerialiable) {
|
||||
BOOST_FOREACH(const Class& cls, expandedClasses) {
|
||||
if(cls.isSerializable)
|
||||
wrapperFile.oss << cls.getSerializationExport() << "\n";
|
||||
}
|
||||
wrapperFile.oss << "\n";
|
||||
}
|
||||
|
||||
// Generate collectors and cleanup function to be called from mexAtExit
|
||||
WriteCollectorsAndCleanupFcn(wrapperFile, name, expandedClasses);
|
||||
|
||||
// generate RTTI registry (for returning derived-most types)
|
||||
WriteRTTIRegistry(wrapperFile, name, expandedClasses);
|
||||
|
||||
vector<string> functionNames; // Function names stored by index for switch
|
||||
|
||||
// create proxy class and wrapper code
|
||||
BOOST_FOREACH(const Class& cls, expandedClasses) {
|
||||
cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
|
||||
|
|
|
|||
|
|
@ -11,6 +11,10 @@
|
|||
%StaticFunctionRet(double z) : returns Point3
|
||||
%staticFunction() : returns double
|
||||
%
|
||||
%-------Serialization Interface-------
|
||||
%string_serialize() : returns string
|
||||
%string_deserialize(string serialized) : returns Point3
|
||||
%
|
||||
classdef Point3 < handle
|
||||
properties
|
||||
ptr_Point3 = 0
|
||||
|
|
@ -49,6 +53,19 @@ classdef Point3 < handle
|
|||
end
|
||||
end
|
||||
|
||||
function varargout = string_serialize(this, varargin)
|
||||
% STRING_SERIALIZE usage: string_serialize() : returns string
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 0
|
||||
varargout{1} = geometry_wrapper(15, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Point3.string_serialize');
|
||||
end
|
||||
end
|
||||
|
||||
function sobj = saveobj(obj)
|
||||
% SAVEOBJ Saves the object to a matlab-readable format
|
||||
sobj = obj.string_serialize();
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
|
|
@ -59,7 +76,7 @@ classdef Point3 < handle
|
|||
% Usage
|
||||
% STATICFUNCTIONRET(double z)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(15, varargin{:});
|
||||
varargout{1} = geometry_wrapper(16, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Point3.StaticFunctionRet');
|
||||
end
|
||||
|
|
@ -72,11 +89,25 @@ classdef Point3 < handle
|
|||
% Usage
|
||||
% STATICFUNCTION()
|
||||
if length(varargin) == 0
|
||||
varargout{1} = geometry_wrapper(16, varargin{:});
|
||||
varargout{1} = geometry_wrapper(17, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Point3.StaticFunction');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = string_deserialize(varargin)
|
||||
% STRING_DESERIALIZE usage: string_deserialize() : returns Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1
|
||||
varargout{1} = geometry_wrapper(18, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Point3.string_deserialize');
|
||||
end
|
||||
end
|
||||
|
||||
function obj = loadobj(sobj)
|
||||
% LOADOBJ Saves the object to a matlab-readable format
|
||||
obj = Point3.string_deserialize(sobj);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
|
|||
|
|
@ -34,11 +34,11 @@ classdef Test < handle
|
|||
function obj = Test(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(17, my_ptr);
|
||||
geometry_wrapper(19, my_ptr);
|
||||
elseif nargin == 0
|
||||
my_ptr = geometry_wrapper(18);
|
||||
my_ptr = geometry_wrapper(20);
|
||||
elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
my_ptr = geometry_wrapper(19, varargin{1}, varargin{2});
|
||||
my_ptr = geometry_wrapper(21, varargin{1}, varargin{2});
|
||||
else
|
||||
error('Arguments do not match any overload of Test constructor');
|
||||
end
|
||||
|
|
@ -46,7 +46,7 @@ classdef Test < handle
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(20, obj.ptr_Test);
|
||||
geometry_wrapper(22, obj.ptr_Test);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
@ -60,7 +60,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% arg_EigenConstRef(Matrix value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(21, this, varargin{:});
|
||||
geometry_wrapper(23, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.arg_EigenConstRef');
|
||||
end
|
||||
|
|
@ -73,7 +73,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% create_MixedPtrs()
|
||||
if length(varargin) == 0
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(22, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.create_MixedPtrs');
|
||||
end
|
||||
|
|
@ -86,7 +86,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% create_ptrs()
|
||||
if length(varargin) == 0
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(23, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.create_ptrs');
|
||||
end
|
||||
|
|
@ -99,7 +99,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% print()
|
||||
if length(varargin) == 0
|
||||
geometry_wrapper(24, this, varargin{:});
|
||||
geometry_wrapper(26, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.print');
|
||||
end
|
||||
|
|
@ -112,7 +112,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_Point2Ptr(bool value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'logical')
|
||||
varargout{1} = geometry_wrapper(25, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(27, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_Point2Ptr');
|
||||
end
|
||||
|
|
@ -125,7 +125,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_Test(Test value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(26, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(28, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_Test');
|
||||
end
|
||||
|
|
@ -138,7 +138,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_TestPtr(Test value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(27, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(29, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_TestPtr');
|
||||
end
|
||||
|
|
@ -151,7 +151,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_bool(bool value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'logical')
|
||||
varargout{1} = geometry_wrapper(28, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(30, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_bool');
|
||||
end
|
||||
|
|
@ -164,7 +164,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_double(double value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(29, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(31, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_double');
|
||||
end
|
||||
|
|
@ -177,7 +177,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_field(Test t)
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(30, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(32, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_field');
|
||||
end
|
||||
|
|
@ -190,7 +190,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_int(int value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'numeric')
|
||||
varargout{1} = geometry_wrapper(31, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(33, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_int');
|
||||
end
|
||||
|
|
@ -203,7 +203,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_matrix1(Matrix value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(32, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(34, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_matrix1');
|
||||
end
|
||||
|
|
@ -216,7 +216,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_matrix2(Matrix value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(33, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(35, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_matrix2');
|
||||
end
|
||||
|
|
@ -229,7 +229,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_pair(Vector v, Matrix A)
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(34, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_pair');
|
||||
end
|
||||
|
|
@ -242,7 +242,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_ptrs(Test p1, Test p2)
|
||||
if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(35, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_ptrs');
|
||||
end
|
||||
|
|
@ -255,7 +255,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_size_t(size_t value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'numeric')
|
||||
varargout{1} = geometry_wrapper(36, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(38, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_size_t');
|
||||
end
|
||||
|
|
@ -268,7 +268,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_string(string value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'char')
|
||||
varargout{1} = geometry_wrapper(37, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(39, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_string');
|
||||
end
|
||||
|
|
@ -281,7 +281,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_vector1(Vector value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(38, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(40, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_vector1');
|
||||
end
|
||||
|
|
@ -294,7 +294,7 @@ classdef Test < handle
|
|||
% Method Overloads
|
||||
% return_vector2(Vector value)
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(39, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(41, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_vector2');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
function varargout = aGlobalFunction(varargin)
|
||||
if length(varargin) == 0
|
||||
varargout{1} = geometry_wrapper(40, varargin{:});
|
||||
varargout{1} = geometry_wrapper(42, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function aGlobalFunction');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -2,9 +2,16 @@
|
|||
#include <map>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <boost/serialization/export.hpp>
|
||||
#include <boost/archive/text_iarchive.hpp>
|
||||
#include <boost/archive/text_oarchive.hpp>
|
||||
|
||||
#include <folder/path/to/Test.h>
|
||||
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(Point2, "Point2");
|
||||
BOOST_CLASS_EXPORT_GUID(Point3, "Point3");
|
||||
|
||||
typedef std::set<boost::shared_ptr<Point2>*> Collector_Point2;
|
||||
static Collector_Point2 collector_Point2;
|
||||
typedef std::set<boost::shared_ptr<Point3>*> Collector_Point3;
|
||||
|
|
@ -220,7 +227,17 @@ void Point3_norm_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]
|
|||
out[0] = wrap< double >(obj->norm());
|
||||
}
|
||||
|
||||
void Point3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Point3_string_serialize_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Point3> Shared;
|
||||
checkArguments("string_serialize",nargout,nargin-1,0);
|
||||
Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
|
||||
std::ostringstream out_archive_stream;
|
||||
boost::archive::text_oarchive out_archive(out_archive_stream);
|
||||
out_archive << *obj;
|
||||
out[0] = wrap< string >(out_archive_stream.str());
|
||||
}
|
||||
void Point3_StaticFunctionRet_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Point3> SharedPoint3;
|
||||
typedef boost::shared_ptr<Point3> Shared;
|
||||
|
|
@ -229,14 +246,25 @@ void Point3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const
|
|||
out[0] = wrap_shared_ptr(SharedPoint3(new Point3(Point3::StaticFunctionRet(z))),"Point3", false);
|
||||
}
|
||||
|
||||
void Point3_staticFunction_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Point3_staticFunction_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Point3> Shared;
|
||||
checkArguments("Point3.staticFunction",nargout,nargin,0);
|
||||
out[0] = wrap< double >(Point3::staticFunction());
|
||||
}
|
||||
|
||||
void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Point3> Shared;
|
||||
checkArguments("Point3.string_deserialize",nargout,nargin,1);
|
||||
string serialized = unwrap< string >(in[0]);
|
||||
std::istringstream in_archive_stream(serialized);
|
||||
boost::archive::text_iarchive in_archive(in_archive_stream);
|
||||
Shared output(new Point3());
|
||||
in_archive >> *output;
|
||||
out[0] = wrap_shared_ptr(output,"Point3", false);
|
||||
}
|
||||
void Test_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
mexAtExit(&_deleteAllObjects);
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
|
|
@ -245,7 +273,7 @@ void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin,
|
|||
collector_Test.insert(self);
|
||||
}
|
||||
|
||||
void Test_constructor_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_constructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
mexAtExit(&_deleteAllObjects);
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
|
|
@ -256,7 +284,7 @@ void Test_constructor_18(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
*reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
|
||||
}
|
||||
|
||||
void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_constructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
mexAtExit(&_deleteAllObjects);
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
|
|
@ -269,7 +297,7 @@ void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
*reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
|
||||
}
|
||||
|
||||
void Test_deconstructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_deconstructor_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("delete_Test",nargout,nargin,1);
|
||||
|
|
@ -282,7 +310,7 @@ void Test_deconstructor_20(int nargout, mxArray *out[], int nargin, const mxArra
|
|||
}
|
||||
}
|
||||
|
||||
void Test_arg_EigenConstRef_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_arg_EigenConstRef_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
|
||||
|
|
@ -291,7 +319,7 @@ void Test_arg_EigenConstRef_21(int nargout, mxArray *out[], int nargin, const mx
|
|||
obj->arg_EigenConstRef(value);
|
||||
}
|
||||
|
||||
void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_create_MixedPtrs_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
|
|
@ -303,7 +331,7 @@ void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxA
|
|||
out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
|
||||
}
|
||||
|
||||
void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_create_ptrs_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
|
|
@ -315,7 +343,7 @@ void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
|
||||
}
|
||||
|
||||
void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_print_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("print",nargout,nargin-1,0);
|
||||
|
|
@ -323,7 +351,7 @@ void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
|||
obj->print();
|
||||
}
|
||||
|
||||
void Test_return_Point2Ptr_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_Point2Ptr_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Point2> SharedPoint2;
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
|
|
@ -333,7 +361,7 @@ void Test_return_Point2Ptr_25(int nargout, mxArray *out[], int nargin, const mxA
|
|||
out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"Point2", false);
|
||||
}
|
||||
|
||||
void Test_return_Test_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_Test_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
|
|
@ -343,7 +371,7 @@ void Test_return_Test_26(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false);
|
||||
}
|
||||
|
||||
void Test_return_TestPtr_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_TestPtr_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
|
|
@ -353,7 +381,7 @@ void Test_return_TestPtr_27(int nargout, mxArray *out[], int nargin, const mxArr
|
|||
out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
|
||||
}
|
||||
|
||||
void Test_return_bool_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_bool_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_bool",nargout,nargin-1,1);
|
||||
|
|
@ -362,7 +390,7 @@ void Test_return_bool_28(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
out[0] = wrap< bool >(obj->return_bool(value));
|
||||
}
|
||||
|
||||
void Test_return_double_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_double_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_double",nargout,nargin-1,1);
|
||||
|
|
@ -371,7 +399,7 @@ void Test_return_double_29(int nargout, mxArray *out[], int nargin, const mxArra
|
|||
out[0] = wrap< double >(obj->return_double(value));
|
||||
}
|
||||
|
||||
void Test_return_field_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_field_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_field",nargout,nargin-1,1);
|
||||
|
|
@ -380,7 +408,7 @@ void Test_return_field_30(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
out[0] = wrap< bool >(obj->return_field(t));
|
||||
}
|
||||
|
||||
void Test_return_int_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_int_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_int",nargout,nargin-1,1);
|
||||
|
|
@ -389,7 +417,7 @@ void Test_return_int_31(int nargout, mxArray *out[], int nargin, const mxArray *
|
|||
out[0] = wrap< int >(obj->return_int(value));
|
||||
}
|
||||
|
||||
void Test_return_matrix1_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_matrix1_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_matrix1",nargout,nargin-1,1);
|
||||
|
|
@ -398,7 +426,7 @@ void Test_return_matrix1_32(int nargout, mxArray *out[], int nargin, const mxArr
|
|||
out[0] = wrap< Matrix >(obj->return_matrix1(value));
|
||||
}
|
||||
|
||||
void Test_return_matrix2_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_matrix2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_matrix2",nargout,nargin-1,1);
|
||||
|
|
@ -407,7 +435,7 @@ void Test_return_matrix2_33(int nargout, mxArray *out[], int nargin, const mxArr
|
|||
out[0] = wrap< Matrix >(obj->return_matrix2(value));
|
||||
}
|
||||
|
||||
void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_pair_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_pair",nargout,nargin-1,2);
|
||||
|
|
@ -419,7 +447,7 @@ void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
out[1] = wrap< Matrix >(pairResult.second);
|
||||
}
|
||||
|
||||
void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_ptrs_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
typedef boost::shared_ptr<Test> SharedTest;
|
||||
|
|
@ -433,7 +461,7 @@ void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray
|
|||
out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
|
||||
}
|
||||
|
||||
void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_size_t_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_size_t",nargout,nargin-1,1);
|
||||
|
|
@ -442,7 +470,7 @@ void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArra
|
|||
out[0] = wrap< size_t >(obj->return_size_t(value));
|
||||
}
|
||||
|
||||
void Test_return_string_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_string_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_string",nargout,nargin-1,1);
|
||||
|
|
@ -451,7 +479,7 @@ void Test_return_string_37(int nargout, mxArray *out[], int nargin, const mxArra
|
|||
out[0] = wrap< string >(obj->return_string(value));
|
||||
}
|
||||
|
||||
void Test_return_vector1_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_vector1_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_vector1",nargout,nargin-1,1);
|
||||
|
|
@ -460,7 +488,7 @@ void Test_return_vector1_38(int nargout, mxArray *out[], int nargin, const mxArr
|
|||
out[0] = wrap< Vector >(obj->return_vector1(value));
|
||||
}
|
||||
|
||||
void Test_return_vector2_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void Test_return_vector2_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
typedef boost::shared_ptr<Test> Shared;
|
||||
checkArguments("return_vector2",nargout,nargin-1,1);
|
||||
|
|
@ -469,7 +497,7 @@ void Test_return_vector2_39(int nargout, mxArray *out[], int nargin, const mxArr
|
|||
out[0] = wrap< Vector >(obj->return_vector2(value));
|
||||
}
|
||||
|
||||
void aGlobalFunction_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
||||
{
|
||||
checkArguments("aGlobalFunction",nargout,nargin,0);
|
||||
out[0] = wrap< Vector >(aGlobalFunction());
|
||||
|
|
@ -532,82 +560,88 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
|
|||
Point3_norm_14(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 15:
|
||||
Point3_StaticFunctionRet_15(nargout, out, nargin-1, in+1);
|
||||
Point3_string_serialize_15(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 16:
|
||||
Point3_staticFunction_16(nargout, out, nargin-1, in+1);
|
||||
Point3_StaticFunctionRet_16(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 17:
|
||||
Test_collectorInsertAndMakeBase_17(nargout, out, nargin-1, in+1);
|
||||
Point3_staticFunction_17(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 18:
|
||||
Test_constructor_18(nargout, out, nargin-1, in+1);
|
||||
Point3_string_deserialize_18(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 19:
|
||||
Test_constructor_19(nargout, out, nargin-1, in+1);
|
||||
Test_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 20:
|
||||
Test_deconstructor_20(nargout, out, nargin-1, in+1);
|
||||
Test_constructor_20(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 21:
|
||||
Test_arg_EigenConstRef_21(nargout, out, nargin-1, in+1);
|
||||
Test_constructor_21(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 22:
|
||||
Test_create_MixedPtrs_22(nargout, out, nargin-1, in+1);
|
||||
Test_deconstructor_22(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 23:
|
||||
Test_create_ptrs_23(nargout, out, nargin-1, in+1);
|
||||
Test_arg_EigenConstRef_23(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 24:
|
||||
Test_print_24(nargout, out, nargin-1, in+1);
|
||||
Test_create_MixedPtrs_24(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 25:
|
||||
Test_return_Point2Ptr_25(nargout, out, nargin-1, in+1);
|
||||
Test_create_ptrs_25(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 26:
|
||||
Test_return_Test_26(nargout, out, nargin-1, in+1);
|
||||
Test_print_26(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 27:
|
||||
Test_return_TestPtr_27(nargout, out, nargin-1, in+1);
|
||||
Test_return_Point2Ptr_27(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 28:
|
||||
Test_return_bool_28(nargout, out, nargin-1, in+1);
|
||||
Test_return_Test_28(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 29:
|
||||
Test_return_double_29(nargout, out, nargin-1, in+1);
|
||||
Test_return_TestPtr_29(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 30:
|
||||
Test_return_field_30(nargout, out, nargin-1, in+1);
|
||||
Test_return_bool_30(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 31:
|
||||
Test_return_int_31(nargout, out, nargin-1, in+1);
|
||||
Test_return_double_31(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 32:
|
||||
Test_return_matrix1_32(nargout, out, nargin-1, in+1);
|
||||
Test_return_field_32(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 33:
|
||||
Test_return_matrix2_33(nargout, out, nargin-1, in+1);
|
||||
Test_return_int_33(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 34:
|
||||
Test_return_pair_34(nargout, out, nargin-1, in+1);
|
||||
Test_return_matrix1_34(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 35:
|
||||
Test_return_ptrs_35(nargout, out, nargin-1, in+1);
|
||||
Test_return_matrix2_35(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 36:
|
||||
Test_return_size_t_36(nargout, out, nargin-1, in+1);
|
||||
Test_return_pair_36(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 37:
|
||||
Test_return_string_37(nargout, out, nargin-1, in+1);
|
||||
Test_return_ptrs_37(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 38:
|
||||
Test_return_vector1_38(nargout, out, nargin-1, in+1);
|
||||
Test_return_size_t_38(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 39:
|
||||
Test_return_vector2_39(nargout, out, nargin-1, in+1);
|
||||
Test_return_string_39(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 40:
|
||||
aGlobalFunction_40(nargout, out, nargin-1, in+1);
|
||||
Test_return_vector1_40(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 41:
|
||||
Test_return_vector2_41(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
case 42:
|
||||
aGlobalFunction_42(nargout, out, nargin-1, in+1);
|
||||
break;
|
||||
}
|
||||
} catch(const std::exception& e) {
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@ class Point2 {
|
|||
void argChar(char a) const;
|
||||
void argUChar(unsigned char a) const;
|
||||
VectorNotEigen vectorConfusion();
|
||||
|
||||
void serializable() const; // Sets flag and creates export, but does not make serialization functions
|
||||
};
|
||||
|
||||
class Point3 {
|
||||
|
|
@ -22,6 +24,9 @@ class Point3 {
|
|||
// static functions - use static keyword and uppercase
|
||||
static double staticFunction();
|
||||
static Point3 StaticFunctionRet(double z);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const; // Just triggers a flag internally and removes actual function
|
||||
};
|
||||
|
||||
// another comment
|
||||
|
|
|
|||
|
|
@ -219,6 +219,10 @@ TEST( wrap, parse_geometry ) {
|
|||
|
||||
EXPECT_LONGS_EQUAL(0, cls.static_methods.size());
|
||||
EXPECT_LONGS_EQUAL(0, cls.namespaces.size());
|
||||
|
||||
// check serialization flag
|
||||
EXPECT(cls.isSerializable);
|
||||
EXPECT(!cls.hasSerialization);
|
||||
}
|
||||
|
||||
// check second class, Point3
|
||||
|
|
@ -251,6 +255,10 @@ TEST( wrap, parse_geometry ) {
|
|||
LONGS_EQUAL(1, m1.argLists.size());
|
||||
EXPECT_LONGS_EQUAL(0, m1.argLists.front().size());
|
||||
EXPECT(m1.is_const_);
|
||||
|
||||
// check serialization flag
|
||||
EXPECT(cls.isSerializable);
|
||||
EXPECT(cls.hasSerialization);
|
||||
}
|
||||
|
||||
// Test class is the third one
|
||||
|
|
|
|||
Loading…
Reference in New Issue