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

Merge (relevant part of) 'fix/matlab_examples_wrapper' into feature/SoundSlam

release/4.3a0
dellaert 2014-12-13 21:00:49 +00:00
parent 2225c10fc0 bcfcf8be8e
commit c22a2d80d2
9 changed files with 127 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam.h
View File

@ -1728,6 +1728,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
@ -2147,7 +2148,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;

4
matlab/gtsam_examples/IMUKittiExampleGPS.m
View File

@ -33,7 +33,7 @@ GPSskip = 10; % Skip this many GPS measurements each time
%% Get initial conditions for the estimated trajectory
currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@ -72,7 +72,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, currentBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
else
t_previous = GPS_data(measurementIndex-1, 1).Time;

4
matlab/gtsam_examples/MonocularVOExample.m
View File

@ -40,7 +40,7 @@ end
%% Create initial estimate
initial = Values;
trueE = EssentialMatrix(aRb,Sphere2(aTb));
trueE = EssentialMatrix(aRb,Unit3(aTb));
initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]');
initial.insert(1, initialE);
@ -52,5 +52,5 @@ result = optimizer.optimize();
%% Print result (as essentialMatrix) and error
error = graph.error(result)
E = result.at(1)
E = result.atEssentialMatrix(1)

18
matlab/gtsam_tests/testPriorFactor.m Normal file
View File

@ -0,0 +1,18 @@
% test wrapping of Values
import gtsam.*;
values = Values;
key = 5;
priorPose3 = Pose3;
model = noiseModel.Unit.Create(6);
factor = PriorFactorPose3(key, priorPose3, model);
values.insert(key, priorPose3);
EXPECT('error', factor.error(values) == 0);
key = 3;
priorVector = [0,0,0]';
model = noiseModel.Unit.Create(3);
factor = PriorFactorVector(key, priorVector, model);
values.insert(key, priorVector);
EXPECT('error', factor.error(values) == 0);

3
matlab/gtsam_tests/test_gtsam.m
View File

@ -1,5 +1,8 @@
% Test runner script - runs each test
% display 'Starting: testPriorFactor'
% testPriorFactor
display 'Starting: testValues'
testValues

6
wrap/Argument.cpp
View File

@ -78,11 +78,11 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
string cppType = type.qualifiedName("::");
string matlabUniqueType = type.qualifiedName();
if (is_ptr)
if (is_ptr && type.category != Qualified::EIGEN)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
file.oss << "boost::shared_ptr<" << cppType << "> " << name
<< " = unwrap_shared_ptr< ";
else if (is_ref)
else if (is_ref && type.category != Qualified::EIGEN)
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";
else
@ -94,7 +94,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << cppType << " " << name << " = unwrap< ";
file.oss << cppType << " >(" << matlabName;
if (is_ptr || is_ref)
if( (is_ptr || is_ref) && type.category != Qualified::EIGEN)
file.oss << ", \"ptr_" << matlabUniqueType << "\"";
file.oss << ");" << endl;
}

17
wrap/matlab.h
View File

@ -425,3 +425,20 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro
boost::shared_ptr<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
return *spp;
}
//// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
//template <>
//Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {
// bool unwrap_shared_ptr_Vector_attempted = false;
// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer");
// return Vector();
//}
//// throw an error if unwrap_shared_ptr is attempted for an Eigen Matrix
//template <>
//Matrix unwrap_shared_ptr<Matrix>(const mxArray* obj, const string& propertyName) {
// bool unwrap_shared_ptr_Matrix_attempted = false;
// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer");
// return Matrix();
//}

2
wrap/tests/expected2/geometry_wrapper.cpp
View File

@ -679,7 +679,7 @@ void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin
typedef boost::shared_ptr<MyTemplatePoint2> Shared;
checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
Matrix& t = unwrap< Matrix >(in[1]);
out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
}

79
wrap/tests/testClass.cpp
View File

@ -160,6 +160,85 @@ TEST( Class, Grammar ) {
EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category);
}
//******************************************************************************
TEST( Class, TemplateSubstition ) {
using classic::space_p;
// Create type grammar that will place result in cls
Class cls;
Template t;
ClassGrammar g(cls, t);
string markup(
string("template<T = {void, double, Matrix, Point3}> class Point2 { \n")
+ string(" T myMethod(const T& t) const; \n")
+ string("};"));
EXPECT(parse(markup.c_str(), g, space_p).full);
// Method 2
Method m2 = cls.method("myMethod");
EXPECT(assert_equal("myMethod", m2.name()));
EXPECT(m2.isConst());
LONGS_EQUAL(1, m2.nrOverloads());
ReturnValue rv2 = m2.returnValue(0);
EXPECT(!rv2.isPair);
EXPECT(!rv2.type1.isPtr);
EXPECT(assert_equal("T", rv2.type1.name()));
EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv2.type1.category);
EXPECT_LONGS_EQUAL(4, t.nrValues());
EXPECT(t.argName()=="T");
EXPECT(t[0]==Qualified("void",Qualified::VOID));
EXPECT(t[1]==Qualified("double",Qualified::BASIS));
EXPECT(t[2]==Qualified("Matrix",Qualified::EIGEN));
EXPECT(t[3]==Qualified("Point3",Qualified::CLASS));
vector<Class> classes = cls.expandTemplate(t.argName(),
t.argValues());
// check the number of new classes is four
EXPECT_LONGS_EQUAL(4, classes.size());
// check return types
EXPECT(classes[0].method("myMethod").returnValue(0).type1 == Qualified("void",Qualified::VOID));
EXPECT(classes[1].method("myMethod").returnValue(0).type1 == Qualified("double",Qualified::BASIS));
EXPECT(classes[2].method("myMethod").returnValue(0).type1 == Qualified("Matrix",Qualified::EIGEN));
EXPECT(classes[3].method("myMethod").returnValue(0).type1 == Qualified("Point3",Qualified::CLASS));
// check the argument types
EXPECT(classes[0].method("myMethod").argumentList(0)[0].type == Qualified("void",Qualified::VOID));
EXPECT(classes[1].method("myMethod").argumentList(0)[0].type == Qualified("double",Qualified::BASIS));
EXPECT(classes[2].method("myMethod").argumentList(0)[0].type == Qualified("Matrix",Qualified::EIGEN));
EXPECT(classes[3].method("myMethod").argumentList(0)[0].type == Qualified("Point3",Qualified::CLASS));
}
TEST(Class, Template) {
using classic::space_p;
// Create type grammar that will place result in cls
Class cls;
Template t;
ClassGrammar g(cls, t);
string markup(
string("template<T = {Vector, Matrix}>"
" virtual class PriorFactor : gtsam::NoiseModelFactor {"
" PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); "
" T prior() const; "
" void serialize() const; "
"};" ));
EXPECT(parse(markup.c_str(), g, space_p).full);
}
/* ************************************************************************* */
int main() {
TestResult tr;