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

Merged in feature/ExpressionExamples (pull request #84) 

Compiled and Move Examples
Fixes issue #198
release/4.3a0
Frank Dellaert 2014-12-28 18:24:45 +01:00
parent 6b47b914b8 3fed29d31e
commit ba6d088fad
6 changed files with 50 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
.cproject
View File

@ -510,22 +510,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SFMExampleExpressions.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SFMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExampleExpressions.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -3086,6 +3070,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExampleExpressions.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>Pose2SLAMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SFMExampleExpressions.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>SFMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

6
gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp → examples/Pose2SLAMExampleExpressions.cpp
View File

@ -18,8 +18,8 @@
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
// Header order is close to far
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -48,6 +48,8 @@ int main(int argc, char** argv) {
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
BinaryExpression<Pose2, Pose2, Pose2>::Function f = traits<Pose2>::Between;
Expression<Pose2>(traits<Pose2>::Between, x1, x2);
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)));

4
gtsam_unstable/examples/SFMExampleExpressions.cpp → examples/SFMExampleExpressions.cpp
View File

@ -23,8 +23,8 @@
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
// Header order is close to far
#include <examples/SFMdata.h>

36
examples/SolverComparer.cpp
View File

@ -72,22 +72,22 @@ typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;
GTSAM_VALUE_EXPORT(Value);
GTSAM_VALUE_EXPORT(Pose);
GTSAM_VALUE_EXPORT(Rot2);
GTSAM_VALUE_EXPORT(Point2);
GTSAM_VALUE_EXPORT(NonlinearFactor);
GTSAM_VALUE_EXPORT(NoiseModelFactor);
GTSAM_VALUE_EXPORT(NM1);
GTSAM_VALUE_EXPORT(NM2);
GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
GTSAM_VALUE_EXPORT(BR);
GTSAM_VALUE_EXPORT(noiseModel::Base);
GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
GTSAM_VALUE_EXPORT(noiseModel::Unit);
//GTSAM_VALUE_EXPORT(Value);
//GTSAM_VALUE_EXPORT(Pose);
//GTSAM_VALUE_EXPORT(Rot2);
//GTSAM_VALUE_EXPORT(Point2);
//GTSAM_VALUE_EXPORT(NonlinearFactor);
//GTSAM_VALUE_EXPORT(NoiseModelFactor);
//GTSAM_VALUE_EXPORT(NM1);
//GTSAM_VALUE_EXPORT(NM2);
//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
//GTSAM_VALUE_EXPORT(BR);
//GTSAM_VALUE_EXPORT(noiseModel::Base);
//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
//GTSAM_VALUE_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
@ -295,7 +295,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@ -474,7 +474,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
gttic_(Create_optimizer);
GaussNewtonParams params;

26
gtsam/base/Lie.h
View File

@ -144,9 +144,6 @@ struct LieGroupTraits : Testable<Class> {
/// @{
typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::identity();}
static Class Compose(const Class& m1, const Class& m2) { return m1 * m2;}
static Class Between(const Class& m1, const Class& m2) { return m1.inverse() * m2;}
static Class Inverse(const Class& m) { return m.inverse();}
/// @}
/// @name Manifold
@ -161,21 +158,13 @@ struct LieGroupTraits : Testable<Class> {
static int GetDimension(const Class&) {return dimension;}
static TangentVector Local(const Class& origin, const Class& other) {
return origin.localCoordinates(other);
}
static Class Retract(const Class& origin, const TangentVector& v) {
return origin.retract(v);
}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin, ChartJacobian Hother = boost::none) {
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother);
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian Horigin, ChartJacobian Hv = boost::none) {
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return origin.retract(v, Horigin, Hv);
}
@ -192,17 +181,18 @@ struct LieGroupTraits : Testable<Class> {
return Class::Expmap(v, Hv);
}
static Class Compose(const Class& m1, const Class& m2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
static Class Compose(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
return m1.compose(m2, H1, H2);
}
static Class Between(const Class& m1, const Class& m2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
static Class Between(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
return m1.between(m2, H1, H2);
}
static Class Inverse(const Class& m, ChartJacobian H) {
static Class Inverse(const Class& m, //
ChartJacobian H = boost::none) {
return m.inverse(H);
}

4
gtsam/nonlinear/expressions.h
View File

@ -8,14 +8,14 @@
#pragma once
#include <gtsam/nonlinear/Expression.h>
#include <boost/bind.hpp>
#include <gtsam/base/Lie.h>
namespace gtsam {
// Generics
template<typename T>
Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
return Expression<T>(t1, &T::between, t2);
return Expression<T>(traits<T>::Between, t1, t2);
}
typedef Expression<double> double_;