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

SmartFactorsCreator + initial fix in kitti example

release/4.3a0
Luca Carlone 2013-10-09 17:13:19 +00:00
parent a7e7da49a5
commit 121e71431a
3 changed files with 226 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

241
gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp
View File

@ -43,6 +43,7 @@
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h> #include <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/SmartProjectionFactor.h> #include <gtsam_unstable/slam/SmartProjectionFactor.h>
#include <gtsam_unstable/slam/SmartProjectionFactorsCreator.h>
// Standard headers, added last, so we know headers above work on their own // Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -61,6 +62,7 @@ using symbol_shorthand::L;
typedef PriorFactor<Pose3> Pose3Prior; typedef PriorFactor<Pose3> Pose3Prior;
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor; typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator;
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor; typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor;
typedef FastMap<Key, boost::shared_ptr<SmartProjectionFactorState> > SmartFactorToStateMap; typedef FastMap<Key, boost::shared_ptr<SmartProjectionFactorState> > SmartFactorToStateMap;
typedef FastMap<Key, boost::shared_ptr<SmartFactor> > SmartFactorMap; typedef FastMap<Key, boost::shared_ptr<SmartFactor> > SmartFactorMap;
@ -72,10 +74,11 @@ bool debug = false;
//// Helper functions taken from VO code //// Helper functions taken from VO code
// Loaded all pose values into list // Loaded all pose values into list
Values::shared_ptr loadPoseValues(const string& filename) { Values::shared_ptr loadPoseValues(const string& filename) {
Values::shared_ptr values(new Values()); Values::shared_ptr values(new Values());
bool addNoise = false; bool addNoise = false;
std::cout << "PARAM Noise: " << addNoise << std::endl; std::cout << "PARAM Noise: " << addNoise << std::endl;
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3));
// read in camera poses // read in camera poses
@ -363,13 +366,13 @@ void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr g
int main(int argc, char** argv) { int main(int argc, char** argv) {
unsigned int maxNumLandmarks = 389007; //100000000; // 309393 // (loop_closure_merged) //37106 //(reduced kitti); unsigned int maxNumLandmarks = 389007; //100000000; // 309393 // (loop_closure_merged) //37106 //(reduced kitti);
unsigned int maxNumPoses = 45400; //3541 unsigned int maxNumPoses = 1e+6;
// Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
bool useSmartProjectionFactor = true; bool useSmartProjectionFactor = true;
bool useTriangulation = true; bool useTriangulation = true;
bool useLM = true; bool useLM = true;
int landmarkFirstOrderingMethod = 2; // 0 - COLAMD, 1 - landmark first, poses from smart factor, 2 - landmark first through constrained ordering int landmarkFirstOrderingMethod = 1; // 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering)
double KittiLinThreshold = -1.0; // 0.005; // double KittiLinThreshold = -1.0; // 0.005; //
double KittiRankTolerance = 1.0; double KittiRankTolerance = 1.0;
@ -420,37 +423,127 @@ int main(int argc, char** argv) {
unsigned int numLandmarks = 0, numPoses = 0; unsigned int numLandmarks = 0, numPoses = 0;
Key r, l; Key r, l;
double uL, uR, v, x, y, z; double uL, uR, v, x, y, z;
std::vector<Key> views;
std::vector<Key> landmarkKeys, cameraPoseKeys; std::vector<Key> landmarkKeys, cameraPoseKeys;
std::vector<Key> views;
std::vector<Point2> measurements; std::vector<Point2> measurements;
Values values; Values values;
SmartFactorToStateMap smartFactorStates;
SmartFactorMap smartFactors;
ProjectionFactorMap projectionFactors; ProjectionFactorMap projectionFactors;
Values result; Values result;
int totalNumMeasurements = 0; int totalNumMeasurements = 0;
bool optimized = false; bool optimized = false;
boost::shared_ptr<Ordering> ordering, landmarkFirstOrdering(new Ordering()); boost::shared_ptr<Ordering> ordering(new Ordering());
SmartFactorsCreator smartCreator(pixel_sigma, K, KittiRankTolerance, KittiLinThreshold);
// main loop: reads measurements and adds factors (also performs optimization if desired) // main loop: reads measurements and adds factors (also performs optimization if desired)
// r >> pose id
// l >> landmark id
// (uL >> uR) >> measurement (xaxis pixel measurement in left and right camera - since we do monocular, we only use uL)
// v >> measurement (yaxis pixel measurement)
// (x >> y >> z) 3D initialization for the landmark (not used in this code)
while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) { while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) {
if (debug) fprintf(stderr,"Landmark %ld\n", l); if (debug) fprintf(stderr,"Landmark %ld\n", l);
if (debug) fprintf(stderr,"Line %d: %d landmarks, (max landmarks %d), %d poses, max poses %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (debug) fprintf(stderr,"Line %d: %d landmarks, (max landmarks %d), %d poses, max poses %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
// 1: add values and factors to the graph
// 1.1: add factors
// SMART FACTORS ..
if (useSmartProjectionFactor) {
smartCreator.add(L(l), X(r), Point2(uL,v), graphSmart);
// Add initial pose value if pose does not exist
if (!graphSmartValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
graphSmartValues->insert(X(r), loadedValues->at<Pose3>(X(r)));
numPoses++;
optimized = false;
}
} else {
// or STANDARD PROJECTION FACTORS
// Create projection factor
ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K));
// Check if landmark exists in mapping
ProjectionFactorMap::iterator pfit = projectionFactors.find(L(l));
if (pfit != projectionFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add projection factor to list of projection factors associated with this landmark
(*pfit).second.push_back(projectionFactor);
} else {
if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end());
// Create a new vector of projection factors
std::vector<ProjectionFactor::shared_ptr> projectionFactorVector;
projectionFactorVector.push_back(projectionFactor);
// Insert projection factor to NEW list of projection factors associated with this landmark
projectionFactors.insert( make_pair(L(l), projectionFactorVector) );
optimized = false; // TODO
// Add projection factor to graph
//graphProjection.push_back(projectionFactor);
// We have a new landmark
//numLandmarks++;
//landmarkKeys.push_back( L(l) );
}
if (!useTriangulation) {
cerr << "Deprecated use of -useTriangulation- flag" << endl;
}
// // Add landmark if triangulation is not being used to initialize them
// if (!useTriangulation) {
// // For projection factor, landmarks positions are used, but have to be transformed to world coordinates
// if (graphProjectionValues->exists<Point3>(L(l)) == boost::none) {
// Pose3 camera = loadedValues->at<Pose3>(X(r));
// Point3 worldPoint = camera.transform_from(Point3(x, y, z));
// graphProjectionValues->insert(L(l), worldPoint); // add point;
// }
//
// // Add initial pose value if pose does not exist
// // Only do this if triangulation is not used. Otherwise, it depends what projection factors are added
// // based on triangulation success
// if (!graphProjectionValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
// graphProjectionValues->insert(X(r), loadedValues->at<Pose3>(X(r)));
// cameraPoseKeys.push_back( X(r) );
// //numPoses++;
// }
//
// // Add projection factor to graph
// graphProjection.push_back(projectionFactor);
//
// }else {
// // Alternatively: Triangulate similar to how SmartProjectionFactor does it
// // We only do this at the end, when all of the camera poses are available
// // Note we do not add anything to the graph until then, since in some cases
// // of triangulation failure we cannot add the landmark to the graph
// }
}
// Optimize if have a certain number of poses/landmarks, or we want to do incremental inference // Optimize if have a certain number of poses/landmarks, or we want to do incremental inference
if (currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks || (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0 )) ) { if (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0) {
if (debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
if (debug) cout << "Adding triangulated landmarks, graph size: " << graphProjection.size() << endl; if (debug) cout << "Adding triangulated landmarks, graph size: " << graphProjection.size() << endl;
//if (useSmartProjectionFactor == false && useTriangulation) { //if (useSmartProjectionFactor == false && useTriangulation) {
addTriangulatedLandmarks(graphProjection, loadedValues, graphProjectionValues, K, projectionFactors, cameraPoseKeys, landmarkKeys); // addTriangulatedLandmarks(graphProjection, loadedValues, graphProjectionValues, K, projectionFactors, cameraPoseKeys, landmarkKeys);
//} //}
if (debug) cout << "Adding triangulated landmarks, graph size after: " << graphProjection.size() << endl; if (debug) cout << "Adding triangulated landmarks, graph size after: " << graphProjection.size() << endl;
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
// Optimize every optSkip poses if we want to do incremental inference // Optimize every optSkip poses if we want to do incremental inference
if (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0 ){ if (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0 ){
// optimize if (useSmartProjectionFactor == false && useTriangulation) {
addTriangulatedLandmarks(graphSmart, loadedValues, graphSmartValues, K, projectionFactors, cameraPoseKeys, landmarkKeys);
}
if (useLM) if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else else
@ -463,32 +556,20 @@ int main(int argc, char** argv) {
if (1||debug) std::cout << "Pose ordering: " << ordering->size() << std::endl; if (1||debug) std::cout << "Pose ordering: " << ordering->size() << std::endl;
if (landmarkFirstOrderingMethod == 1) { if (landmarkFirstOrderingMethod == 1) {
// Add landmark keys first for ordering
BOOST_FOREACH(const Key& key, landmarkKeys) {
landmarkFirstOrdering->push_back(key);
}
// Add COLAMD on pose keys to ordering
//Ordering::iterator oit;
BOOST_FOREACH(const Key& key, *ordering) {
landmarkFirstOrdering->push_back(key);
}
} else if (landmarkFirstOrderingMethod == 2) {
OrderingMap orderingMap; OrderingMap orderingMap;
// Add landmark keys first for ordering // Add landmark keys first for ordering
BOOST_FOREACH(const Key& key, landmarkKeys) { BOOST_FOREACH(const Key& key, landmarkKeys) {
orderingMap.insert( make_pair(key, 1) ); orderingMap.insert( make_pair(key, 1) );
} }
//Ordering::iterator oit; //Ordering::iterator oit;
BOOST_FOREACH(const Key& key, *ordering) { BOOST_FOREACH(const Key& key, cameraPoseKeys) {
orderingMap.insert( make_pair(key, 2) ); orderingMap.insert( make_pair(key, 2) );
} }
*landmarkFirstOrdering = graphProjection.orderingCOLAMDConstrained(orderingMap); *ordering = graphProjection.orderingCOLAMDConstrained(orderingMap);
} }
if (1||debug) std::cout << "Optimizing landmark first " << landmarkFirstOrdering->size() << std::endl; if (1||debug) std::cout << "Optimizing landmark first " << ordering->size() << std::endl;
optimizeGraphLM(graphProjection, graphProjectionValues, result, landmarkFirstOrdering); optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
// Only process first N measurements (for development/debugging) // Only process first N measurements (for development/debugging)
if ( (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks) ) { if ( (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks) ) {
@ -498,109 +579,7 @@ int main(int argc, char** argv) {
if(!incrementalFlag) break; if(!incrementalFlag) break;
} }
// add factors
// SMART FACTORS ..
if (useSmartProjectionFactor) {
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(L(l));
SmartFactorMap::iterator fit = smartFactors.find(L(l));
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add measurement to smart factor
(*fit).second->add(Point2(uL,v), X(r));
totalNumMeasurements++;
if (debug) (*fit).second->print();
} else {
if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end());
views += X(r);
measurements += Point2(uL,v);
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionFactorState> smartFactorState(new SmartProjectionFactorState());
//SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K));
SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K, KittiRankTolerance, KittiLinThreshold));
smartFactorStates.insert( make_pair(L(l), smartFactorState) );
smartFactors.insert( make_pair(L(l), smartFactor) );
graphSmart.push_back(smartFactor);
numLandmarks++;
//landmarkKeys.push_back( L(l) );
totalNumMeasurements++;
views.clear();
measurements.clear();
}
// Add initial pose value if pose does not exist
if (!graphSmartValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
graphSmartValues->insert(X(r), loadedValues->at<Pose3>(X(r)));
numPoses++;
optimized = false;
}
} else {
// or STANDARD PROJECTION FACTORS
// Create projection factor
ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K));
// Check if landmark exists in mapping
ProjectionFactorMap::iterator pfit = projectionFactors.find(L(l));
if (pfit != projectionFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add projection factor to list of projection factors associated with this landmark
(*pfit).second.push_back(projectionFactor);
} else {
if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end());
// Create a new vector of projection factors
std::vector<ProjectionFactor::shared_ptr> projectionFactorVector;
projectionFactorVector.push_back(projectionFactor);
// Insert projection factor to NEW list of projection factors associated with this landmark
projectionFactors.insert( make_pair(L(l), projectionFactorVector) );
// Add projection factor to graph
//graphProjection.push_back(projectionFactor);
// We have a new landmark
//numLandmarks++;
//landmarkKeys.push_back( L(l) );
}
// Add landmark if triangulation is not being used to initialize them
if (!useTriangulation) {
// For projection factor, landmarks positions are used, but have to be transformed to world coordinates
if (graphProjectionValues->exists<Point3>(L(l)) == boost::none) {
Pose3 camera = loadedValues->at<Pose3>(X(r));
Point3 worldPoint = camera.transform_from(Point3(x, y, z));
graphProjectionValues->insert(L(l), worldPoint); // add point;
}
// Add initial pose value if pose does not exist
// Only do this if triangulation is not used. Otherwise, it depends what projection factors are added
// based on triangulation success
if (!graphProjectionValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
graphProjectionValues->insert(X(r), loadedValues->at<Pose3>(X(r)));
cameraPoseKeys.push_back( X(r) );
//numPoses++;
}
// Add projection factor to graph
graphProjection.push_back(projectionFactor);
}else {
// Alternatively: Triangulate similar to how SmartProjectionFactor does it
// We only do this at the end, when all of the camera poses are available
// Note we do not add anything to the graph until then, since in some cases
// of triangulation failure we cannot add the landmark to the graph
}
}
if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks); if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks);
if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl;
@ -613,7 +592,11 @@ int main(int argc, char** argv) {
} }
} }
} if(currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks)){ // reached desired number of landmarks/poses
break;
}
} // end of while
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);

4
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -196,8 +196,8 @@ namespace gtsam {
boost::optional<POSE> body_P_sensor = boost::none, boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
measured_(measured), noise_(model), K_(K), measured_(measured), noise_(model), K_(K),
retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol),
linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor),
state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) { state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {
} }

112
gtsam_unstable/slam/SmartProjectionFactorsCreator.h Normal file
View File

@ -0,0 +1,112 @@
/*
* SmartProjectionFactorsCreator.h
*
* Created on: Oct 8, 2013
* Author: aspn
*/
#ifndef SMARTPROJECTIONFACTORSCREATOR_H_
#define SMARTPROJECTIONFACTORSCREATOR_H_
// Both relative poses and recovered trajectory poses will be stored as Pose3 objects
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h>
#include <gtsam_unstable/slam/SmartProjectionFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
//#include <boost/foreach.hpp>
//#include <boost/assign.hpp>
//#include <boost/assign/std/vector.hpp>
//#include <fstream>
//#include <iostream>
#include <utility>
namespace gtsam {
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
typedef FastMap<Key, boost::shared_ptr<SmartProjectionFactorState> > SmartFactorToStateMap;
typedef FastMap<Key, boost::shared_ptr<SmartFactor> > SmartFactorMap;
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class SmartProjectionFactorsCreator {
public:
SmartProjectionFactorsCreator(const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
const double rankTol,
const double linThreshold,
boost::optional<POSE> body_P_sensor = boost::none) :
noise_(model), K_(K), rankTolerance_(rankTol),
linearizationThreshold_(linThreshold), body_P_sensor_(body_P_sensor),
totalNumMeasurements(0), numLandmarks(0) {};
void add(Key landmarkKey,
Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) {
std::vector<Key> views;
std::vector<Point2> measurements;
bool debug = false;
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey);
SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add measurement to smart factor
(*fit).second->add(measurement, poseKey);
totalNumMeasurements++;
if (debug) (*fit).second->print();
} else {
if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end());
views += poseKey;
measurements += measurement;
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionFactorState> smartFactorState(new SmartProjectionFactorState());
SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, noise_, K_, rankTolerance_, linearizationThreshold_));
smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
graph.push_back(smartFactor);
numLandmarks++;
//landmarkKeys.push_back( L(l) );
totalNumMeasurements++;
views.clear();
measurements.clear();
}
}
protected:
const SharedNoiseModel noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor)
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
double rankTolerance_; ///< threshold to decide whether triangulation is degenerate
double linearizationThreshold_; ///< threshold to decide whether to re-linearize
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
SmartFactorToStateMap smartFactorStates;
SmartFactorMap smartFactors;
int totalNumMeasurements;
int numLandmarks;
};
}
#endif /* SMARTPROJECTIONFACTORSCREATOR_H_ */