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 "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h"
using namespace std;
namespace gtsam {
// 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,
int nrFrames, double sigma,
const gtsam::Cal3_S2 &K)
{
ifstream ifs(path.c_str(), ios::in);
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);
bool compareLandmark(const std::string& key,
const VSLAMConfig& feasible,
const VSLAMConfig& input) {
int j = atoi(key.substr(1, key.size() - 1).c_str());
return feasible.landmarkPoint(j).equals(input.landmarkPoint(j));
}
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 "VSLAMConfig.h"
using namespace std;
namespace gtsam{
/**
@ -40,14 +39,6 @@ public:
*/
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
*/
@ -55,11 +46,17 @@ public:
gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
}
void load_dumped(const std::string& path);
// Getters
int Get_nFrames(){return nFrames;};
int Get_nFeat_ids(){return feat_ids.size();};
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