35 lines
997 B
C++
35 lines
997 B
C++
/**
|
|
* @file Pose2Graph.cpp
|
|
* @brief A factor graph for the 2D PoseSLAM problem
|
|
* @authors Frank Dellaert, Viorela Ila
|
|
*/
|
|
|
|
#include "Pose2Graph.h"
|
|
#include "FactorGraph-inl.h"
|
|
#include "NonlinearFactorGraph-inl.h"
|
|
#include "NonlinearOptimizer-inl.h"
|
|
#include "NonlinearEquality.h"
|
|
#include "graph-inl.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
namespace gtsam {
|
|
|
|
// explicit instantiation so all the code is there and we can link with it
|
|
template class FactorGraph<NonlinearFactor<Pose2Config> > ;
|
|
template class NonlinearFactorGraph<Pose2Config> ;
|
|
template class NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2> ;
|
|
template class NonlinearOptimizer<Pose2Graph, Pose2Config>;
|
|
|
|
void Pose2Graph::addConstraint(const Pose2Config::Key& key, const Pose2& pose) {
|
|
push_back(sharedFactor(new NonlinearEquality<Pose2Config, Pose2Config::Key,
|
|
Pose2> (key, pose)));
|
|
}
|
|
|
|
bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
|
|
return false;
|
|
}
|
|
|
|
} // namespace gtsam
|