UrbanFactor, UrbanGraph.cpp, template instantiations
Frank Dellaert
parent
7d0de77fc6
commit
cf1fde7bda
|
|
@ -189,8 +189,7 @@ testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
|
|||
testPose3Factor_LDADD = libgtsam.la
|
||||
|
||||
# Urban
|
||||
headers += UrbanGraph.h
|
||||
sources += UrbanConfig.cpp UrbanMeasurement.cpp
|
||||
sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanGraph.cpp
|
||||
check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanGraph
|
||||
testUrbanConfig_SOURCES = $(example) testUrbanConfig.cpp
|
||||
testUrbanConfig_LDADD = libgtsam.la
|
||||
|
|
|
|||
|
|
@ -5,14 +5,11 @@
|
|||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include "VectorConfig.h"
|
||||
#include "Pose3.h"
|
||||
#include "Point2.h"
|
||||
#include "Testable.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "Point2.h"
|
||||
#include "Pose3.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,21 @@
|
|||
/*
|
||||
* UrbanFactor.cpp
|
||||
*
|
||||
* Created on: Dec 17, 2009
|
||||
* Author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "UrbanFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
UrbanFactor::UrbanFactor() {
|
||||
// TODO Auto-generated constructor stub
|
||||
|
||||
}
|
||||
|
||||
UrbanFactor::~UrbanFactor() {
|
||||
// TODO Auto-generated destructor stub
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
* UrbanFactor.h
|
||||
*
|
||||
* Created on: Dec 17, 2009
|
||||
* Author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#ifndef URBANFACTOR_H_
|
||||
#define URBANFACTOR_H_
|
||||
|
||||
#include "NonlinearFactor.h"
|
||||
#include "UrbanConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class UrbanFactor : public NonlinearFactor<UrbanConfig> {
|
||||
public:
|
||||
UrbanFactor();
|
||||
virtual ~UrbanFactor();
|
||||
|
||||
/** Vector of errors */
|
||||
Vector error_vector(const UrbanConfig& c) const { return zero(0); }
|
||||
|
||||
/** linearize to a GaussianFactor */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const UrbanConfig& c) const {
|
||||
boost::shared_ptr<GaussianFactor> factor(new GaussianFactor);
|
||||
return factor;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* URBANFACTOR_H_ */
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/**
|
||||
* @file UrbanGraph.cpp
|
||||
* @brief A factor graph for the Urban problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "UrbanGraph.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// explicit instantiation so all the code is there and we can link with it
|
||||
template class FactorGraph<UrbanFactor>;
|
||||
template class NonlinearFactorGraph<UrbanConfig>;
|
||||
template class NonlinearOptimizer<UrbanGraph,UrbanConfig>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
UrbanGraph::UrbanGraph() {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanGraph::print(const std::string& s) const {
|
||||
gtsam::NonlinearFactorGraph<UrbanConfig>::print(s);
|
||||
// TODO
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool UrbanGraph::equals(const UrbanGraph& p, double tol) const {
|
||||
return gtsam::NonlinearFactorGraph<UrbanConfig>::equals(p, tol);
|
||||
// TODO
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanGraph::addMeasurement(double x, double y, double sigma, int p1,
|
||||
int p2) {
|
||||
// TODO
|
||||
}
|
||||
;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx,
|
||||
double sigmayaw, int p) {
|
||||
// TODO
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanGraph::addOriginConstraint(int p) {
|
||||
// TODO
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -4,66 +4,56 @@
|
|||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <fstream>
|
||||
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "UrbanMeasurement.h"
|
||||
#include "Pose3Factor.h"
|
||||
#include "UrbanConfig.h"
|
||||
#include "Testable.h"
|
||||
#include "UrbanFactor.h"
|
||||
|
||||
namespace gtsam{
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
/**
|
||||
* Non-linear factor graph for visual SLAM
|
||||
*/
|
||||
class UrbanGraph : public gtsam::NonlinearFactorGraph<UrbanConfig>{
|
||||
class UrbanGraph: public gtsam::NonlinearFactorGraph<UrbanConfig> {
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** default constructor is empty graph */
|
||||
UrbanGraph() {}
|
||||
UrbanGraph();
|
||||
|
||||
/**
|
||||
* print out graph
|
||||
*/
|
||||
void print(const std::string& s = "") const {
|
||||
gtsam::NonlinearFactorGraph<UrbanConfig>::print(s);
|
||||
}
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const UrbanGraph& p, double tol=1e-9) const {
|
||||
return gtsam::NonlinearFactorGraph<UrbanConfig>::equals(p, tol);
|
||||
}
|
||||
// TODO implement addMeasurenment, addOdometry and addOriginalConstraint (out of the class)
|
||||
bool equals(const UrbanGraph& p, double tol = 1e-9) const;
|
||||
|
||||
/**
|
||||
* Add a landmark constraint
|
||||
*/
|
||||
void addMeasurement(double x, double y, double sigma, int p1, int p2) {};
|
||||
void addMeasurement(double x, double y, double sigma, int p1, int p2);
|
||||
|
||||
/**
|
||||
* Add an odometry constraint
|
||||
*/
|
||||
void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int p ) {};
|
||||
void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw,
|
||||
int p);
|
||||
|
||||
/**
|
||||
* Add an initial constraint on the first pose
|
||||
*/
|
||||
void addOriginConstraint(int p){};
|
||||
void addOriginConstraint(int p);
|
||||
|
||||
private:
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {}
|
||||
};
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -23,9 +23,6 @@ template class FactorGraph<VSLAMFactor>;
|
|||
template class NonlinearFactorGraph<VSLAMConfig>;
|
||||
template class NonlinearOptimizer<VSLAMGraph,VSLAMConfig>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool compareLandmark(const std::string& key,
|
||||
const VSLAMConfig& feasible,
|
||||
|
|
|
|||
|
|
@ -13,9 +13,8 @@ using namespace boost::assign;
|
|||
#include "UrbanGraph.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "Pose3.h"
|
||||
#include "Point2.h"
|
||||
#include "Ordering.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
typedef NonlinearOptimizer<UrbanGraph,UrbanConfig> Optimizer;
|
||||
|
|
|
|||
Loading…
Reference in New Issue