64 lines
2.3 KiB
C++
64 lines
2.3 KiB
C++
/*
|
|
* visualSLAM.cpp
|
|
*
|
|
* Created on: Jan 14, 2010
|
|
* Author: richard
|
|
*/
|
|
|
|
#include "visualSLAM.h"
|
|
#include "TupleConfig-inl.h"
|
|
#include "NonlinearOptimizer-inl.h"
|
|
#include "NonlinearFactorGraph-inl.h"
|
|
|
|
namespace gtsam {
|
|
|
|
INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3)
|
|
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config)
|
|
INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config)
|
|
|
|
namespace visualSLAM {
|
|
|
|
/* ************************************************************************* */
|
|
void ProjectionFactor::print(const std::string& s) const {
|
|
Base::print(s);
|
|
z_.print(s + ".z");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const {
|
|
return Base::equals(p, tol) && z_.equals(p.z_, tol)
|
|
&& K_->equals(*p.K_, tol);
|
|
}
|
|
|
|
// /* ************************************************************************* */
|
|
// 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[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]);
|
|
// }
|
|
//
|
|
// /* ************************************************************************* */
|
|
// void VSLAMGraph::addLandmarkConstraint(int j, const Point3& p) {
|
|
// typedef NonlinearEquality<VSLAMConfig,VSLAMPointKey,Point3> NLE;
|
|
// boost::shared_ptr<NLE> factor(new NLE(j, p));
|
|
// push_back(factor);
|
|
// }
|
|
//
|
|
// /* ************************************************************************* */
|
|
// bool compareCamera(const std::string& key,
|
|
// const VSLAMConfig& feasible,
|
|
// const VSLAMConfig& input) {
|
|
// int j = atoi(key.substr(1, key.size() - 1).c_str());
|
|
// return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]);
|
|
// }
|
|
//
|
|
// /* ************************************************************************* */
|
|
// void VSLAMGraph::addCameraConstraint(int j, const Pose3& p) {
|
|
// typedef NonlinearEquality<VSLAMConfig,VSLAMPoseKey,Pose3> NLE;
|
|
// boost::shared_ptr<NLE> factor(new NLE(j,p));
|
|
// push_back(factor);
|
|
// }
|
|
}
|
|
}
|