37 lines
1.1 KiB
C++
37 lines
1.1 KiB
C++
/**
|
|
* @file Pose3Graph.cpp
|
|
* @brief A factor graph for the 2D PoseSLAM problem
|
|
* @authors Frank Dellaert, Viorela Ila
|
|
*/
|
|
|
|
#include "Pose3Graph.h"
|
|
#include "FactorGraph-inl.h"
|
|
#include "NonlinearFactorGraph-inl.h"
|
|
#include "NonlinearOptimizer-inl.h"
|
|
#include "NonlinearEquality.h"
|
|
#include "graph-inl.h"
|
|
|
|
namespace gtsam {
|
|
|
|
// explicit instantiation so all the code is there and we can link with it
|
|
template class FactorGraph<NonlinearFactor<gtsam::Pose3Config> > ;
|
|
template class NonlinearFactorGraph<Pose3Config> ;
|
|
template class NonlinearEquality<Pose3Config, Pose3Config::Key, Pose3> ;
|
|
template class NonlinearOptimizer<Pose3Graph, Pose3Config> ;
|
|
|
|
bool Pose3Graph::equals(const Pose3Graph& p, double tol) const {
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* Add an equality constraint on a pose
|
|
* @param key of pose
|
|
* @param pose which pose to constrain it to
|
|
*/
|
|
void Pose3Graph::addConstraint(const Pose3Config::Key& key, const Pose3& pose) {
|
|
push_back(sharedFactor(new NonlinearEquality<Pose3Config, Pose3Config::Key,
|
|
Pose3> (key, pose)));
|
|
}
|
|
|
|
} // namespace gtsam
|