41 lines
1.2 KiB
C++
41 lines
1.2 KiB
C++
/**
|
|
* @file VSLAMFactor.cpp
|
|
* @brief A non-linear factor specialized to the Visual SLAM problem
|
|
* @author Alireza Fathi
|
|
*/
|
|
|
|
#include <boost/bind.hpp>
|
|
#include <boost/bind/placeholders.hpp>
|
|
|
|
#include "VSLAMFactor.h"
|
|
#include "SimpleCamera.h"
|
|
|
|
using namespace std;
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
VSLAMFactor::VSLAMFactor() {
|
|
/// Arbitrary values
|
|
K_ = shared_ptrK(new Cal3_S2(444, 555, 666, 777, 888));
|
|
}
|
|
/* ************************************************************************* */
|
|
VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln,
|
|
const shared_ptrK &K) :
|
|
VSLAMFactorBase(sigma, cn, ln), z_(z), K_(K) {
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void VSLAMFactor::print(const std::string& s) const {
|
|
VSLAMFactorBase::print(s);
|
|
z_.print(s + ".z");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
|
|
return VSLAMFactorBase::equals(p, tol) && z_.equals(p.z_, tol)
|
|
&& K_->equals(*p.K_, tol);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
} // namespace gtsam
|