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

addLandmarkConstraint

release/4.3a0
Frank Dellaert 2009-11-13 06:18:27 +00:00
parent 309f2151cf
commit d26abf3ccf
2 changed files with 24 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
cpp/VSLAMGraph.cpp
View File

@ -12,8 +12,10 @@
#include "VSLAMGraph.h" #include "VSLAMGraph.h"
#include "NonlinearFactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h"
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
// explicit instantiation so all the code is there and we can link with it // explicit instantiation so all the code is there and we can link with it
@ -76,32 +78,20 @@ VSLAMGraph::VSLAMGraph(const std::string& path)
} }
/* ************************************************************************* */ /* ************************************************************************* */
VSLAMGraph::VSLAMGraph(const std::string& path, bool compareLandmark(const std::string& key,
int nrFrames, double sigma, const VSLAMConfig& feasible,
const gtsam::Cal3_S2 &K) const VSLAMConfig& input) {
{ int j = atoi(key.substr(1, key.size() - 1).c_str());
ifstream ifs(path.c_str(), ios::in); return feasible.landmarkPoint(j).equals(input.landmarkPoint(j));
if(ifs) {
int cameraFrameNumber, landmarkNumber;
double landmarkX, landmarkY, landmarkZ;
double u, v;
ifs >> cameraFrameNumber >> landmarkNumber >> u >> v >> landmarkX >> landmarkY >> landmarkZ;
//Store the measurements
Vector z(2);
z(0)=u;
z(1)=v;
//VSLAMFactor::shared_ptr f1(new VSLAMFactor<VSLAMConfig>::VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
//factors_.push_back(f1);
}
else {
printf("Unable to load values in %s\n", path.c_str());
exit(0);
} }
ifs.close(); /* ************************************************************************* */
void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
typedef NonlinearEquality<VSLAMConfig> NLE;
VSLAMConfig feasible;
feasible.addLandmarkPoint(j,p);
boost::shared_ptr<NLE> factor(new NLE(symbol('l',j), feasible, 3, compareLandmark));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */

19
cpp/VSLAMGraph.h
View File

@ -17,7 +17,6 @@
#include "VSLAMFactor.h" #include "VSLAMFactor.h"
#include "VSLAMConfig.h" #include "VSLAMConfig.h"
using namespace std;
namespace gtsam{ namespace gtsam{
/** /**
@ -40,14 +39,6 @@ public:
*/ */
VSLAMGraph(const std::string& path); VSLAMGraph(const std::string& path);
/**
* Constructor that loads from VO file (not tested)
* @param path to the file
* @param nrFrames the number of frames to load
* @return new factor graph
*/
VSLAMGraph(const std::string& path, int nrFrames, double sigma, const gtsam::Cal3_S2& K);
/** /**
* print out graph * print out graph
*/ */
@ -55,11 +46,17 @@ public:
gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s); gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
} }
void load_dumped(const std::string& path); // Getters
int Get_nFrames(){return nFrames;}; int Get_nFrames(){return nFrames;};
int Get_nFeat_ids(){return feat_ids.size();}; int Get_nFeat_ids(){return feat_ids.size();};
feat_ids_type* Get_feat_ids_map(){return &feat_ids;}; feat_ids_type* Get_feat_ids_map(){return &feat_ids;};
/**
* Add a constraint on a landmark (for now, *must* be satisfied in any Config)
* @param j index of landmark
* @param p to which point to constrain it to
*/
void addLandmarkConstraint(int j, const Point3& p = Point3());
}; };
} // namespace gtsam } // namespace gtsam