Renamed BADFactor -> ExpressionFactor
dellaert
parent
7e069191e5
commit
563c4d214c
102
.cproject
102
.cproject
|
|
@ -600,6 +600,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -607,6 +608,7 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -654,6 +656,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -661,6 +664,7 @@
|
|||
</target>
|
||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -668,6 +672,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -683,6 +688,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1034,6 +1040,7 @@
|
|||
</target>
|
||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1263,6 +1270,46 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1345,7 +1392,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1385,7 +1431,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1393,7 +1438,6 @@
|
|||
</target>
|
||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1407,46 +1451,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1704,6 +1708,7 @@
|
|||
</target>
|
||||
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G DEB</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1711,6 +1716,7 @@
|
|||
</target>
|
||||
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G RPM</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1718,6 +1724,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G TGZ</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1725,6 +1732,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2451,6 +2459,7 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2458,6 +2467,7 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2465,6 +2475,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2534,10 +2545,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testBADFactor.run</buildTarget>
|
||||
<buildTarget>testExpressionFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2952,7 +2963,6 @@
|
|||
</target>
|
||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
// The two new headers that allow using our Automatic Differentiation Expression framework
|
||||
#include <gtsam_unstable/slam/expressions.h>
|
||||
#include <gtsam_unstable/nonlinear/BADFactor.h>
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
|
||||
// Header order is close to far
|
||||
#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
|
||||
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
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
graph.push_back(BADFactor<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(BADFactor<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, 0 ), between(x1,x2)));
|
||||
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
|
||||
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
|
||||
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x4,x5)));
|
||||
|
||||
// 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
|
||||
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
|
||||
// The two new headers that allow using our Automatic Differentiation Expression framework
|
||||
#include <gtsam_unstable/slam/expressions.h>
|
||||
#include <gtsam_unstable/nonlinear/BADFactor.h>
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
|
||||
// Header order is close to far
|
||||
#include <examples/SFMdata.h>
|
||||
|
|
@ -57,10 +57,10 @@ int main(int argc, char* argv[]) {
|
|||
// Specify uncertainty on first pose prior
|
||||
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]
|
||||
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
|
||||
Cal3_S2_ cK(K);
|
||||
|
|
@ -74,14 +74,14 @@ int main(int argc, char* argv[]) {
|
|||
// Below an expression for the prediction of the measurement:
|
||||
Point3_ p('l', j);
|
||||
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
|
||||
// Again, here we use a BADFactor
|
||||
graph.push_back(BADFactor<Point2>(measurementNoise, measurement, prediction));
|
||||
// Again, here we use a ExpressionFactor
|
||||
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);
|
||||
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
|
||||
Values initialEstimate;
|
||||
|
|
|
|||
|
|
@ -24,10 +24,10 @@
|
|||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* BAD Factor that supports arbitrary expressions via AD
|
||||
* Factor that supports arbitrary expressions via AD
|
||||
*/
|
||||
template<class T>
|
||||
class BADFactor: public NoiseModelFactor {
|
||||
class ExpressionFactor: public NoiseModelFactor {
|
||||
|
||||
const T measurement_;
|
||||
const Expression<T> expression_;
|
||||
|
|
@ -35,7 +35,7 @@ class BADFactor: public NoiseModelFactor {
|
|||
public:
|
||||
|
||||
/// Constructor
|
||||
BADFactor(const SharedNoiseModel& noiseModel, //
|
||||
ExpressionFactor(const SharedNoiseModel& noiseModel, //
|
||||
const T& measurement, const Expression<T>& expression) :
|
||||
NoiseModelFactor(noiseModel, expression.keys()), //
|
||||
measurement_(measurement), expression_(expression) {
|
||||
|
|
@ -61,7 +61,7 @@ public:
|
|||
}
|
||||
|
||||
};
|
||||
// BADFactor
|
||||
// ExpressionFactor
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -77,7 +77,7 @@ TEST(Expression, leaf) {
|
|||
// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
|
||||
//}
|
||||
/* ************************************************************************* */
|
||||
|
||||
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
|
||||
TEST(Expression, test) {
|
||||
|
||||
// Test Constant expression
|
||||
|
|
@ -95,7 +95,7 @@ TEST(Expression, test) {
|
|||
Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
|
||||
|
||||
// Check keys
|
||||
std::set<Key> expectedKeys;
|
||||
set<Key> expectedKeys;
|
||||
expectedKeys.insert(1);
|
||||
expectedKeys.insert(2);
|
||||
expectedKeys.insert(3);
|
||||
|
|
@ -111,7 +111,7 @@ TEST(Expression, compose1) {
|
|||
Expression<Rot3> R3 = R1 * R2;
|
||||
|
||||
// Check keys
|
||||
std::set<Key> expectedKeys;
|
||||
set<Key> expectedKeys;
|
||||
expectedKeys.insert(1);
|
||||
expectedKeys.insert(2);
|
||||
EXPECT(expectedKeys == R3.keys());
|
||||
|
|
@ -126,7 +126,7 @@ TEST(Expression, compose2) {
|
|||
Expression<Rot3> R3 = R1 * R2;
|
||||
|
||||
// Check keys
|
||||
std::set<Key> expectedKeys;
|
||||
set<Key> expectedKeys;
|
||||
expectedKeys.insert(1);
|
||||
EXPECT(expectedKeys == R3.keys());
|
||||
}
|
||||
|
|
@ -140,7 +140,7 @@ TEST(Expression, compose3) {
|
|||
Expression<Rot3> R3 = R1 * R2;
|
||||
|
||||
// Check keys
|
||||
std::set<Key> expectedKeys;
|
||||
set<Key> expectedKeys;
|
||||
expectedKeys.insert(3);
|
||||
EXPECT(expectedKeys == R3.keys());
|
||||
}
|
||||
|
|
@ -167,7 +167,7 @@ TEST(Expression, ternary) {
|
|||
Expression<Rot3> ABC(composeThree, A, B, C);
|
||||
|
||||
// Check keys
|
||||
std::set<Key> expectedKeys;
|
||||
set<Key> expectedKeys;
|
||||
expectedKeys.insert(1);
|
||||
expectedKeys.insert(2);
|
||||
expectedKeys.insert(3);
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testBADFactor.cpp
|
||||
* @file testExpressionFactor.cpp
|
||||
* @date September 18, 2014
|
||||
* @author Frank Dellaert
|
||||
* @author Paul Furgale
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
*/
|
||||
|
||||
#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/ProjectionFactor.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
|
@ -35,7 +35,7 @@ SharedNoiseModel model = noiseModel::Unit::Create(2);
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Leaf
|
||||
TEST(BADFactor, leaf) {
|
||||
TEST(ExpressionFactor, leaf) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -49,7 +49,7 @@ TEST(BADFactor, leaf) {
|
|||
Point2_ p(2);
|
||||
|
||||
// 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());
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
|
|
@ -59,7 +59,7 @@ TEST(BADFactor, leaf) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// non-zero noise model
|
||||
TEST(BADFactor, model) {
|
||||
TEST(ExpressionFactor, model) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -75,7 +75,7 @@ TEST(BADFactor, model) {
|
|||
// Try concise version
|
||||
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());
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
|
|
@ -85,7 +85,7 @@ TEST(BADFactor, model) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Unary(Leaf))
|
||||
TEST(BADFactor, test) {
|
||||
TEST(ExpressionFactor, test) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -99,7 +99,7 @@ TEST(BADFactor, test) {
|
|||
Point3_ p(2);
|
||||
|
||||
// Try concise version
|
||||
BADFactor<Point2> f(model, measured, project(p));
|
||||
ExpressionFactor<Point2> f(model, measured, project(p));
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
|
|
@ -109,7 +109,7 @@ TEST(BADFactor, test) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Unary(Binary(Leaf,Leaf))
|
||||
TEST(BADFactor, test1) {
|
||||
TEST(ExpressionFactor, test1) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -127,7 +127,7 @@ TEST(BADFactor, test1) {
|
|||
Point3_ p(2);
|
||||
|
||||
// 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_LONGS_EQUAL(2, f2.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
|
||||
|
|
@ -136,7 +136,7 @@ TEST(BADFactor, test1) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
|
||||
TEST(BADFactor, test2) {
|
||||
TEST(ExpressionFactor, test2) {
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -160,14 +160,14 @@ TEST(BADFactor, test2) {
|
|||
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
|
||||
|
||||
// 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_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
|
||||
EXPECT( assert_equal(*expected, *gf, 1e-9));
|
||||
|
||||
// Try concise version
|
||||
BADFactor<Point2> f2(model, measured,
|
||||
ExpressionFactor<Point2> f2(model, measured,
|
||||
uncalibrate(K, project(transform_to(x, p))));
|
||||
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f2.dim());
|
||||
|
|
@ -177,7 +177,7 @@ TEST(BADFactor, test2) {
|
|||
TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6;
|
||||
|
||||
// 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_LONGS_EQUAL(2, f3.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
|
||||
|
|
@ -186,14 +186,14 @@ TEST(BADFactor, test2) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST(BADFactor, compose1) {
|
||||
TEST(ExpressionFactor, compose1) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(2);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -216,14 +216,14 @@ TEST(BADFactor, compose1) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with arguments referring to the same rotation
|
||||
TEST(BADFactor, compose2) {
|
||||
TEST(ExpressionFactor, compose2) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(1), R2(1);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -245,14 +245,14 @@ TEST(BADFactor, compose2) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Test compose with one arguments referring to a constant same rotation
|
||||
TEST(BADFactor, compose3) {
|
||||
TEST(ExpressionFactor, compose3) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ R1(Rot3::identity()), R2(3);
|
||||
Rot3_ R3 = R1 * R2;
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -287,14 +287,14 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
|
|||
return R1 * (R2 * R3);
|
||||
}
|
||||
|
||||
TEST(BADFactor, composeTernary) {
|
||||
TEST(ExpressionFactor, composeTernary) {
|
||||
|
||||
// Create expression
|
||||
Rot3_ A(1), B(2), C(3);
|
||||
Rot3_ ABC(composeThree, A, B, C);
|
||||
|
||||
// Create factor
|
||||
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
|
||||
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
|
||||
|
||||
// Create some values
|
||||
Values values;
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
*/
|
||||
|
||||
#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/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
|
@ -76,17 +76,17 @@ int main() {
|
|||
GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3);
|
||||
time("GeneralSFMFactor2<Cal3_S2> : ", f1, values);
|
||||
|
||||
// BADFactor
|
||||
// ExpressionFactor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 20.3 musecs/call
|
||||
BADFactor<Point2> f2(model, z,
|
||||
ExpressionFactor<Point2> f2(model, z,
|
||||
uncalibrate(K, project(transform_to(x, p))));
|
||||
time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values);
|
||||
|
||||
// BADFactor ternary
|
||||
// ExpressionFactor ternary
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 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);
|
||||
|
||||
// CALIBRATED
|
||||
|
|
@ -97,19 +97,19 @@ int main() {
|
|||
GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK);
|
||||
time("GenericProjectionFactor<P,P>: ", g1, values);
|
||||
|
||||
// BADFactor
|
||||
// ExpressionFactor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 16.0 musecs/call
|
||||
BADFactor<Point2> g2(model, z,
|
||||
ExpressionFactor<Point2> g2(model, z,
|
||||
uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p))));
|
||||
time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values);
|
||||
|
||||
// BADFactor, optimized
|
||||
// ExpressionFactor, optimized
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 9.0 musecs/call
|
||||
typedef PinholeCamera<Cal3_S2> 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);
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@
|
|||
*/
|
||||
|
||||
#include <gtsam_unstable/slam/expressions.h>
|
||||
#include <gtsam_unstable/nonlinear/BADFactor.h>
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
|
||||
#include <time.h>
|
||||
#include <iostream>
|
||||
|
|
@ -55,14 +55,14 @@ int main() {
|
|||
values.insert(2, Point3(0, 0, 1));
|
||||
values.insert(3, Cal3_S2());
|
||||
|
||||
// BADFactor
|
||||
// ExpressionFactor
|
||||
// Oct 3, 2014, Macbook Air
|
||||
// 20.3 musecs/call
|
||||
//#define TERNARY
|
||||
#ifdef TERNARY
|
||||
BADFactor<Point2> f(model, z, project3(x, p, K));
|
||||
ExpressionFactor<Point2> f(model, z, project3(x, p, K));
|
||||
#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
|
||||
time(f, values);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue