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

New routines to create Key collections

release/4.3a0
dellaert 2014-05-25 11:15:17 -04:00
parent 7aa0bd332a
commit ddcf9c0efb
2 changed files with 57 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam.h
View File

@ -2363,6 +2363,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
namespace utilities { namespace utilities {
#include <matlab.h> #include <matlab.h>
gtsam::KeyList createKeyList(Vector I);
gtsam::KeyList createKeyList(string s, Vector I);
gtsam::KeyVector createKeyVector(Vector I);
gtsam::KeyVector createKeyVector(string s, Vector I);
gtsam::KeySet createKeySet(Vector I);
gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values);
@ -2375,7 +2381,6 @@ namespace utilities {
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
gtsam::KeySet createKeySet(string s, Vector I);
} //\namespace utilities } //\namespace utilities

60
matlab.h
View File

@ -34,6 +34,57 @@ namespace gtsam {
namespace utilities { namespace utilities {
// Create a KeyList from indices
FastList<Key> createKeyList(const Vector& I) {
FastList<Key> set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyList from indices using symbol
FastList<Key> createKeyList(string s, const Vector& I) {
FastList<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
return set;
}
// Create a KeyVector from indices
FastVector<Key> createKeyVector(const Vector& I) {
FastVector<Key> set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyVector from indices using symbol
FastVector<Key> createKeyVector(string s, const Vector& I) {
FastVector<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
return set;
}
// Create a KeySet from indices
FastSet<Key> createKeySet(const Vector& I) {
FastSet<Key> set;
for (int i = 0; i < I.size(); i++)
set.insert(I[i]);
return set;
}
// Create a KeySet from indices using symbol
FastSet<Key> createKeySet(string s, const Vector& I) {
FastSet<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));
return set;
}
/// Extract all Point2 values into a single matrix [x y] /// Extract all Point2 values into a single matrix [x y]
Matrix extractPoint2(const Values& values) { Matrix extractPoint2(const Values& values) {
size_t j = 0; size_t j = 0;
@ -167,15 +218,6 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
return errors; return errors;
} }
// Create a Keyset from indices
FastSet<Key> createKeySet(string s, const Vector& I) {
FastSet<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));
return set;
}
} }
} }