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

Overload for insertProjectionFactors in matlab utilities

release/4.3a0
Andrew Melim 2012-12-17 22:30:54 +00:00
parent 682daa3e61
commit 93b59990e3
2 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam.h
View File

@ -84,38 +84,44 @@
* - TODO: Handle gtsam::Rot3M conversions to quaternions * - TODO: Handle gtsam::Rot3M conversions to quaternions
*/ */
/*namespace std { namespace std {
#include <vector>
template<T> template<T>
//Module.cpp needs to be changed to allow lowercase classnames
class vector class vector
{ {
//Do we need these?
//Capacity //Capacity
size_t size() const; /*size_t size() const;
size_t max_size() const; size_t max_size() const;
void resize(size_t sz, T c = T()); //void resize(size_t sz);
size_t capacity() const; size_t capacity() const;
bool empty() const; bool empty() const;
void reserve(size_t n); void reserve(size_t n);
//Element acces //Element acces
T& at(size_t n); T* at(size_t n);
const T& at(size_t n) const; T* front();
T& front(); T* back();
const T& front() const;
T& back();
const T& back() const;
//Modifiers //Modifiers
void assign(size_t n, const T& u); void assign(size_t n, const T& u);
void push_back(const T& x); void push_back(const T& x);
void pop_back(); void pop_back();*/
}; };
}*/ //typedef std::vector
#include<list>
template<T>
class list
{
};
}
namespace gtsam { namespace gtsam {
//typedef std::vector<Index> IndexVector;
//************************************************************************* //*************************************************************************
// base // base
//************************************************************************* //*************************************************************************
@ -255,6 +261,7 @@ virtual class Point2 : gtsam::Value {
double y() const; double y() const;
Vector vector() const; Vector vector() const;
double dist(const gtsam::Point2& p2) const; double dist(const gtsam::Point2& p2) const;
double norm() const;
}; };
virtual class StereoPoint2 : gtsam::Value { virtual class StereoPoint2 : gtsam::Value {
@ -844,6 +851,7 @@ virtual class BayesTreeClique {
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
typedef gtsam::BayesNet<gtsam::IndexConditional> SymbolicBayesNetBase; typedef gtsam::BayesNet<gtsam::IndexConditional> SymbolicBayesNetBase;
virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
SymbolicBayesNet(); SymbolicBayesNet();
@ -1943,6 +1951,8 @@ template<POSE, LANDMARK, CALIBRATION>
virtual class GenericProjectionFactor : gtsam::NonlinearFactor { virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t pointKey, const CALIBRATION* k); size_t poseKey, size_t pointKey, const CALIBRATION* k);
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
gtsam::Point2 measured() const; gtsam::Point2 measured() const;
CALIBRATION* calibration() const; CALIBRATION* calibration() const;
}; };
@ -2006,6 +2016,7 @@ namespace utilities {
void perturbPoint3(gtsam::Values& values, double sigma, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed);
void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
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);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
} //\namespace utilities } //\namespace utilities

4
matlab.h
View File

@ -114,14 +114,14 @@ namespace gtsam {
/// insert multiple projection factors for a single pose key /// insert multiple projection factors for a single pose key
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K) { const SharedNoiseModel& model, const shared_ptrK K, const Pose3& body_P_sensor = Pose3()) {
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument( if (Z.cols() != J.size()) throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries"); "addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) { for (int k = 0; k < Z.cols(); k++) {
graph.push_back( graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> > boost::make_shared<GenericProjectionFactor<Pose3, Point3> >
(Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K)); (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
} }
} }