Merge (relevant part of) 'fix/matlab_examples_wrapper' into feature/SoundSlam
						commit
						c22a2d80d2
					
				
							
								
								
									
										3
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										3
									
								
								gtsam.h
								
								
								
								
							|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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) | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  | @ -1,5 +1,8 @@ | |||
| % Test runner script - runs each test  | ||||
| 
 | ||||
| % display 'Starting: testPriorFactor' | ||||
| % testPriorFactor | ||||
| 
 | ||||
| display 'Starting: testValues' | ||||
| testValues | ||||
| 
 | ||||
|  |  | |||
|  | @ -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; | ||||
| } | ||||
|  |  | |||
|  | @ -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();
 | ||||
| //}
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue