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

Renamed BADFactor -> ExpressionFactor

release/4.3a0
dellaert 2014-10-09 13:00:56 +02:00
parent 7e069191e5
commit 563c4d214c
8 changed files with 115 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
.cproject
View File

@ -600,6 +600,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -607,6 +608,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -654,6 +656,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -661,6 +664,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -668,6 +672,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -683,6 +688,7 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1034,6 +1040,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1263,6 +1270,46 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1345,7 +1392,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1385,7 +1431,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1393,7 +1438,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1407,46 +1451,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1704,6 +1708,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1711,6 +1716,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1718,6 +1724,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1725,6 +1732,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2451,6 +2459,7 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2458,6 +2467,7 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2465,6 +2475,7 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2534,10 +2545,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBADFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testExpressionFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testBADFactor.run</buildTarget> <buildTarget>testExpressionFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2952,7 +2963,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>

14
gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp
View File

@ -19,7 +19,7 @@
// The two new headers that allow using our Automatic Differentiation Expression framework // The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h> #include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/BADFactor.h> #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
// Header order is close to far // Header order is close to far
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -42,19 +42,19 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin // 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(BADFactor<Pose2>(priorNoise, Pose2(0, 0, 0), x1)); graph.push_back(ExpressionFactor<Pose2>(priorNoise, Pose2(0, 0, 0), x1));
// For simplicity, we will use the same noise model for odometry and loop closures // For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors // 2b. Add odometry factors
graph.push_back(BADFactor<Pose2>(model, Pose2(2, 0, 0 ), between(x1,x2))); graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, 0 ), between(x1,x2)));
graph.push_back(BADFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3))); graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
graph.push_back(BADFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4))); graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
graph.push_back(BADFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x4,x5))); graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x4,x5)));
// 2c. Add the loop closure constraint // 2c. Add the loop closure constraint
graph.push_back(BADFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x5,x2))); graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x5,x2)));
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution // 3. Create the data structure to hold the initialEstimate estimate to the solution

14
gtsam_unstable/examples/SFMExampleExpressions.cpp
View File

@ -24,7 +24,7 @@
// The two new headers that allow using our Automatic Differentiation Expression framework // The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h> #include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/BADFactor.h> #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
// Header order is close to far // Header order is close to far
#include <examples/SFMdata.h> #include <examples/SFMdata.h>
@ -57,10 +57,10 @@ int main(int argc, char* argv[]) {
// Specify uncertainty on first pose prior // Specify uncertainty on first pose prior
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
// Here we don't use a PriorFactor but directly the BADFactor class // Here we don't use a PriorFactor but directly the ExpressionFactor class
// The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
Pose3_ x0('x',0); Pose3_ x0('x',0);
graph.push_back(BADFactor<Pose3>(poseNoise, poses[0], x0)); graph.push_back(ExpressionFactor<Pose3>(poseNoise, poses[0], x0));
// We create a constant Expression for the calibration here // We create a constant Expression for the calibration here
Cal3_S2_ cK(K); Cal3_S2_ cK(K);
@ -74,14 +74,14 @@ int main(int argc, char* argv[]) {
// Below an expression for the prediction of the measurement: // Below an expression for the prediction of the measurement:
Point3_ p('l', j); Point3_ p('l', j);
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
// Again, here we use a BADFactor // Again, here we use a ExpressionFactor
graph.push_back(BADFactor<Point2>(measurementNoise, measurement, prediction)); graph.push_back(ExpressionFactor<Point2>(measurementNoise, measurement, prediction));
} }
} }
// Add prior on first point to constrain scale, again with BADFActor // Add prior on first point to constrain scale, again with ExpressionFactor
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(BADFactor<Point3>(pointNoise, points[0], Point3_('l', 0))); graph.push_back(ExpressionFactor<Point3>(pointNoise, points[0], Point3_('l', 0)));
// Create perturbed initial // Create perturbed initial
Values initialEstimate; Values initialEstimate;

8
gtsam_unstable/nonlinear/BADFactor.h → gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -24,10 +24,10 @@
namespace gtsam { namespace gtsam {
/** /**
* BAD Factor that supports arbitrary expressions via AD * Factor that supports arbitrary expressions via AD
*/ */
template<class T> template<class T>
class BADFactor: public NoiseModelFactor { class ExpressionFactor: public NoiseModelFactor {
const T measurement_; const T measurement_;
const Expression<T> expression_; const Expression<T> expression_;
@ -35,7 +35,7 @@ class BADFactor: public NoiseModelFactor {
public: public:
/// Constructor /// Constructor
BADFactor(const SharedNoiseModel& noiseModel, // ExpressionFactor(const SharedNoiseModel& noiseModel, //
const T& measurement, const Expression<T>& expression) : const T& measurement, const Expression<T>& expression) :
NoiseModelFactor(noiseModel, expression.keys()), // NoiseModelFactor(noiseModel, expression.keys()), //
measurement_(measurement), expression_(expression) { measurement_(measurement), expression_(expression) {
@ -61,7 +61,7 @@ public:
} }
}; };
// BADFactor // ExpressionFactor
} }

12
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -77,7 +77,7 @@ TEST(Expression, leaf) {
// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
TEST(Expression, test) { TEST(Expression, test) {
// Test Constant expression // Test Constant expression
@ -95,7 +95,7 @@ TEST(Expression, test) {
Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection); Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
// Check keys // Check keys
std::set<Key> expectedKeys; set<Key> expectedKeys;
expectedKeys.insert(1); expectedKeys.insert(1);
expectedKeys.insert(2); expectedKeys.insert(2);
expectedKeys.insert(3); expectedKeys.insert(3);
@ -111,7 +111,7 @@ TEST(Expression, compose1) {
Expression<Rot3> R3 = R1 * R2; Expression<Rot3> R3 = R1 * R2;
// Check keys // Check keys
std::set<Key> expectedKeys; set<Key> expectedKeys;
expectedKeys.insert(1); expectedKeys.insert(1);
expectedKeys.insert(2); expectedKeys.insert(2);
EXPECT(expectedKeys == R3.keys()); EXPECT(expectedKeys == R3.keys());
@ -126,7 +126,7 @@ TEST(Expression, compose2) {
Expression<Rot3> R3 = R1 * R2; Expression<Rot3> R3 = R1 * R2;
// Check keys // Check keys
std::set<Key> expectedKeys; set<Key> expectedKeys;
expectedKeys.insert(1); expectedKeys.insert(1);
EXPECT(expectedKeys == R3.keys()); EXPECT(expectedKeys == R3.keys());
} }
@ -140,7 +140,7 @@ TEST(Expression, compose3) {
Expression<Rot3> R3 = R1 * R2; Expression<Rot3> R3 = R1 * R2;
// Check keys // Check keys
std::set<Key> expectedKeys; set<Key> expectedKeys;
expectedKeys.insert(3); expectedKeys.insert(3);
EXPECT(expectedKeys == R3.keys()); EXPECT(expectedKeys == R3.keys());
} }
@ -167,7 +167,7 @@ TEST(Expression, ternary) {
Expression<Rot3> ABC(composeThree, A, B, C); Expression<Rot3> ABC(composeThree, A, B, C);
// Check keys // Check keys
std::set<Key> expectedKeys; set<Key> expectedKeys;
expectedKeys.insert(1); expectedKeys.insert(1);
expectedKeys.insert(2); expectedKeys.insert(2);
expectedKeys.insert(3); expectedKeys.insert(3);

44
gtsam_unstable/nonlinear/tests/testBADFactor.cpp → gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testBADFactor.cpp * @file testExpressionFactor.cpp
* @date September 18, 2014 * @date September 18, 2014
* @author Frank Dellaert * @author Frank Dellaert
* @author Paul Furgale * @author Paul Furgale
@ -18,7 +18,7 @@
*/ */
#include <gtsam_unstable/slam/expressions.h> #include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/BADFactor.h> #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h> #include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
@ -35,7 +35,7 @@ SharedNoiseModel model = noiseModel::Unit::Create(2);
/* ************************************************************************* */ /* ************************************************************************* */
// Leaf // Leaf
TEST(BADFactor, leaf) { TEST(ExpressionFactor, leaf) {
// Create some values // Create some values
Values values; Values values;
@ -49,7 +49,7 @@ TEST(BADFactor, leaf) {
Point2_ p(2); Point2_ p(2);
// Try concise version // Try concise version
BADFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
@ -59,7 +59,7 @@ TEST(BADFactor, leaf) {
/* ************************************************************************* */ /* ************************************************************************* */
// non-zero noise model // non-zero noise model
TEST(BADFactor, model) { TEST(ExpressionFactor, model) {
// Create some values // Create some values
Values values; Values values;
@ -75,7 +75,7 @@ TEST(BADFactor, model) {
// Try concise version // Try concise version
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
BADFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
@ -85,7 +85,7 @@ TEST(BADFactor, model) {
/* ************************************************************************* */ /* ************************************************************************* */
// Unary(Leaf)) // Unary(Leaf))
TEST(BADFactor, test) { TEST(ExpressionFactor, test) {
// Create some values // Create some values
Values values; Values values;
@ -99,7 +99,7 @@ TEST(BADFactor, test) {
Point3_ p(2); Point3_ p(2);
// Try concise version // Try concise version
BADFactor<Point2> f(model, measured, project(p)); ExpressionFactor<Point2> f(model, measured, project(p));
EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
@ -109,7 +109,7 @@ TEST(BADFactor, test) {
/* ************************************************************************* */ /* ************************************************************************* */
// Unary(Binary(Leaf,Leaf)) // Unary(Binary(Leaf,Leaf))
TEST(BADFactor, test1) { TEST(ExpressionFactor, test1) {
// Create some values // Create some values
Values values; Values values;
@ -127,7 +127,7 @@ TEST(BADFactor, test1) {
Point3_ p(2); Point3_ p(2);
// Try concise version // Try concise version
BADFactor<Point2> f2(model, measured, project(transform_to(x, p))); ExpressionFactor<Point2> f2(model, measured, project(transform_to(x, p)));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim()); EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
@ -136,7 +136,7 @@ TEST(BADFactor, test1) {
/* ************************************************************************* */ /* ************************************************************************* */
// Binary(Leaf,Unary(Binary(Leaf,Leaf))) // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
TEST(BADFactor, test2) { TEST(ExpressionFactor, test2) {
// Create some values // Create some values
Values values; Values values;
@ -160,14 +160,14 @@ TEST(BADFactor, test2) {
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Create factor and check value, dimension, linearization // Create factor and check value, dimension, linearization
BADFactor<Point2> f(model, measured, uv_hat); ExpressionFactor<Point2> f(model, measured, uv_hat);
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9)); EXPECT( assert_equal(*expected, *gf, 1e-9));
// Try concise version // Try concise version
BADFactor<Point2> f2(model, measured, ExpressionFactor<Point2> f2(model, measured,
uncalibrate(K, project(transform_to(x, p)))); uncalibrate(K, project(transform_to(x, p))));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim()); EXPECT_LONGS_EQUAL(2, f2.dim());
@ -177,7 +177,7 @@ TEST(BADFactor, test2) {
TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6; TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6;
// Try ternary version // Try ternary version
BADFactor<Point2> f3(model, measured, project3(x, p, K)); ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f3.dim()); EXPECT_LONGS_EQUAL(2, f3.dim());
boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values); boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
@ -186,14 +186,14 @@ TEST(BADFactor, test2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(BADFactor, compose1) { TEST(ExpressionFactor, compose1) {
// Create expression // Create expression
Rot3_ R1(1), R2(2); Rot3_ R1(1), R2(2);
Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
// Create factor // Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values // Create some values
Values values; Values values;
@ -216,14 +216,14 @@ TEST(BADFactor, compose1) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test compose with arguments referring to the same rotation // Test compose with arguments referring to the same rotation
TEST(BADFactor, compose2) { TEST(ExpressionFactor, compose2) {
// Create expression // Create expression
Rot3_ R1(1), R2(1); Rot3_ R1(1), R2(1);
Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
// Create factor // Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values // Create some values
Values values; Values values;
@ -245,14 +245,14 @@ TEST(BADFactor, compose2) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation // Test compose with one arguments referring to a constant same rotation
TEST(BADFactor, compose3) { TEST(ExpressionFactor, compose3) {
// Create expression // Create expression
Rot3_ R1(Rot3::identity()), R2(3); Rot3_ R1(Rot3::identity()), R2(3);
Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
// Create factor // Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Create some values // Create some values
Values values; Values values;
@ -287,14 +287,14 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
return R1 * (R2 * R3); return R1 * (R2 * R3);
} }
TEST(BADFactor, composeTernary) { TEST(ExpressionFactor, composeTernary) {
// Create expression // Create expression
Rot3_ A(1), B(2), C(3); Rot3_ A(1), B(2), C(3);
Rot3_ ABC(composeThree, A, B, C); Rot3_ ABC(composeThree, A, B, C);
// Create factor // Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC); ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
// Create some values // Create some values
Values values; Values values;

18
gtsam_unstable/timing/timeCameraExpression.cpp
View File

@ -17,7 +17,7 @@
*/ */
#include <gtsam_unstable/slam/expressions.h> #include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/BADFactor.h> #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/ProjectionFactor.h> #include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
@ -76,17 +76,17 @@ int main() {
GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3); GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3);
time("GeneralSFMFactor2<Cal3_S2> : ", f1, values); time("GeneralSFMFactor2<Cal3_S2> : ", f1, values);
// BADFactor // ExpressionFactor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 20.3 musecs/call // 20.3 musecs/call
BADFactor<Point2> f2(model, z, ExpressionFactor<Point2> f2(model, z,
uncalibrate(K, project(transform_to(x, p)))); uncalibrate(K, project(transform_to(x, p))));
time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values);
// BADFactor ternary // ExpressionFactor ternary
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 20.3 musecs/call // 20.3 musecs/call
BADFactor<Point2> f3(model, z, project3(x, p, K)); ExpressionFactor<Point2> f3(model, z, project3(x, p, K));
time("Ternary(Leaf,Leaf,Leaf) : ", f3, values); time("Ternary(Leaf,Leaf,Leaf) : ", f3, values);
// CALIBRATED // CALIBRATED
@ -97,19 +97,19 @@ int main() {
GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK); GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK);
time("GenericProjectionFactor<P,P>: ", g1, values); time("GenericProjectionFactor<P,P>: ", g1, values);
// BADFactor // ExpressionFactor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 16.0 musecs/call // 16.0 musecs/call
BADFactor<Point2> g2(model, z, ExpressionFactor<Point2> g2(model, z,
uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p))));
time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values); time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values);
// BADFactor, optimized // ExpressionFactor, optimized
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 9.0 musecs/call // 9.0 musecs/call
typedef PinholeCamera<Cal3_S2> Camera; typedef PinholeCamera<Cal3_S2> Camera;
typedef Expression<Camera> Camera_; typedef Expression<Camera> Camera_;
BADFactor<Point2> g3(model, z, Point2_(myProject, x, p)); ExpressionFactor<Point2> g3(model, z, Point2_(myProject, x, p));
time("Binary(Leaf,Leaf) : ", g3, values); time("Binary(Leaf,Leaf) : ", g3, values);
return 0; return 0;
} }

8
gtsam_unstable/timing/timeOneCameraExpression.cpp
View File

@ -17,7 +17,7 @@
*/ */
#include <gtsam_unstable/slam/expressions.h> #include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/BADFactor.h> #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <time.h> #include <time.h>
#include <iostream> #include <iostream>
@ -55,14 +55,14 @@ int main() {
values.insert(2, Point3(0, 0, 1)); values.insert(2, Point3(0, 0, 1));
values.insert(3, Cal3_S2()); values.insert(3, Cal3_S2());
// BADFactor // ExpressionFactor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 20.3 musecs/call // 20.3 musecs/call
//#define TERNARY //#define TERNARY
#ifdef TERNARY #ifdef TERNARY
BADFactor<Point2> f(model, z, project3(x, p, K)); ExpressionFactor<Point2> f(model, z, project3(x, p, K));
#else #else
BADFactor<Point2> f(model, z, uncalibrate(K, project(transform_to(x, p)))); ExpressionFactor<Point2> f(model, z, uncalibrate(K, project(transform_to(x, p))));
#endif #endif
time(f, values); time(f, values);