moved all "Urban" factors and tests to (cmake-based) CitySLAM project
Frank Dellaert
parent
603b692406
commit
ff06cd757f
|
|
@ -199,18 +199,6 @@ testPose2Graph_LDADD = libgtsam.la
|
|||
testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
|
||||
testPose3Factor_LDADD = libgtsam.la
|
||||
|
||||
# Urban
|
||||
#sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanOdometry.cpp UrbanGraph.cpp
|
||||
#check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanOdometry testUrbanGraph
|
||||
#testUrbanConfig_SOURCES = $(example) testUrbanConfig.cpp
|
||||
#testUrbanConfig_LDADD = libgtsam.la
|
||||
#testUrbanMeasurement_SOURCES = $(example) testUrbanMeasurement.cpp
|
||||
#testUrbanMeasurement_LDADD = libgtsam.la
|
||||
#testUrbanOdometry_SOURCES = $(example) testUrbanOdometry.cpp
|
||||
#testUrbanOdometry_LDADD = libgtsam.la
|
||||
#testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp
|
||||
#testUrbanGraph_LDADD = libgtsam.la
|
||||
|
||||
# Robot Control example system
|
||||
sources += ControlConfig.cpp ControlPoint.cpp ControlGraph.cpp
|
||||
check_PROGRAMS += testControlConfig testControlPoint testControlGraph
|
||||
|
|
|
|||
|
|
@ -1,115 +0,0 @@
|
|||
/**
|
||||
* @file UrbanConfig.cpp
|
||||
* @brief The Config for Urban example
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
#include "UrbanConfig.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
#define SIGMA 1.0
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Exponential map
|
||||
UrbanConfig UrbanConfig::exmap(const VectorConfig & delta) const {
|
||||
|
||||
UrbanConfig newConfig;
|
||||
|
||||
for (map<string, Vector>::const_iterator it = delta.begin(); it
|
||||
!= delta.end(); it++) {
|
||||
string key = it->first;
|
||||
if (key[0] == 'x') {
|
||||
int robotNumber = atoi(key.substr(1, key.size() - 1).c_str());
|
||||
if (robotPoseExists(robotNumber)) {
|
||||
Pose3 basePose = robotPose(robotNumber);
|
||||
newConfig.addRobotPose(robotNumber, basePose.exmap(it->second));
|
||||
}
|
||||
}
|
||||
if (key[0] == 'l') {
|
||||
int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str());
|
||||
if (landmarkPointExists(landmarkNumber)) {
|
||||
Point2 basePoint = landmarkPoint(landmarkNumber);
|
||||
newConfig.addLandmark(landmarkNumber, basePoint.exmap(it->second));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return newConfig;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanConfig::print(const std::string& s) const
|
||||
{
|
||||
printf("%s:\n", s.c_str());
|
||||
printf("robot Poses:\n");
|
||||
for(PoseMap::const_iterator it = robotIteratorBegin(); it != robotIteratorEnd(); it++)
|
||||
{
|
||||
printf("x%d:\n", it->first);
|
||||
it->second.print();
|
||||
}
|
||||
printf("Landmark Points:\n");
|
||||
for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++)
|
||||
{
|
||||
printf("l%d:\n", (*it).first);
|
||||
(*it).second.print();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool UrbanConfig::equals(const UrbanConfig& c, double tol) const {
|
||||
for (PoseMap::const_iterator it = robotIteratorBegin(); it
|
||||
!= robotIteratorEnd(); it++) {
|
||||
if (!c.robotPoseExists(it->first)) return false;
|
||||
if (!it->second.equals(c.robotPose(it->first), tol)) return false;
|
||||
}
|
||||
|
||||
for (PointMap::const_iterator it = landmarkIteratorBegin(); it
|
||||
!= landmarkIteratorEnd(); it++) {
|
||||
if (!c.landmarkPointExists(it->first)) return false;
|
||||
if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanConfig::addRobotPose(const int i, Pose3 cp)
|
||||
{
|
||||
robotPoses_.insert(make_pair(i,cp));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanConfig::addLandmark(const int j, Point2 lp)
|
||||
{
|
||||
landmarkPoints_.insert(make_pair(j,lp));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanConfig::removeRobotPose(const int i)
|
||||
{
|
||||
if(robotPoseExists(i))
|
||||
robotPoses_.erase(i);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanConfig::removeLandmark(const int i)
|
||||
{
|
||||
if(landmarkPointExists(i))
|
||||
landmarkPoints_.erase(i);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -1,134 +0,0 @@
|
|||
/**
|
||||
* @file UrbanConfig.h
|
||||
* @brief Config for Urban application
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include "Testable.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "Point2.h"
|
||||
#include "Pose3.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Config that knows about points and poses
|
||||
*/
|
||||
class UrbanConfig: Testable<UrbanConfig> {
|
||||
|
||||
private:
|
||||
typedef std::map<int, Pose3> PoseMap;
|
||||
typedef std::map<int, Point2> PointMap;
|
||||
PointMap landmarkPoints_;
|
||||
PoseMap robotPoses_;
|
||||
|
||||
static std::string symbol(char c, int index) {
|
||||
std::stringstream ss;
|
||||
ss << c << index;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
public:
|
||||
typedef std::map<std::string, Vector>::const_iterator const_iterator;
|
||||
typedef PoseMap::const_iterator const_Pose_iterator;
|
||||
typedef PointMap::const_iterator const_Point_iterator;
|
||||
/**
|
||||
* default constructor
|
||||
*/
|
||||
UrbanConfig() {
|
||||
}
|
||||
|
||||
/*
|
||||
* copy constructor
|
||||
*/
|
||||
UrbanConfig(const UrbanConfig& original) :
|
||||
landmarkPoints_(original.landmarkPoints_),
|
||||
robotPoses_(original.robotPoses_)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Exponential map: takes 6D vectors in VectorConfig
|
||||
* and applies them to the poses in the UrbanConfig.
|
||||
* Needed for use in nonlinear optimization
|
||||
*/
|
||||
UrbanConfig exmap(const VectorConfig & delta) const;
|
||||
|
||||
PoseMap::const_iterator robotIteratorBegin() const {
|
||||
return robotPoses_.begin();
|
||||
}
|
||||
PoseMap::const_iterator robotIteratorEnd() const {
|
||||
return robotPoses_.end();
|
||||
}
|
||||
PointMap::const_iterator landmarkIteratorBegin() const {
|
||||
return landmarkPoints_.begin();
|
||||
}
|
||||
PointMap::const_iterator landmarkIteratorEnd() const {
|
||||
return landmarkPoints_.end();
|
||||
}
|
||||
|
||||
/**
|
||||
* print
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/**
|
||||
* Retrieve robot pose
|
||||
*/
|
||||
bool robotPoseExists(int i) const {
|
||||
PoseMap::const_iterator it = robotPoses_.find(i);
|
||||
return (it != robotPoses_.end());
|
||||
}
|
||||
|
||||
Pose3 robotPose(int i) const {
|
||||
PoseMap::const_iterator it = robotPoses_.find(i);
|
||||
if (it == robotPoses_.end()) throw(std::invalid_argument(
|
||||
"robotPose: invalid key " + symbol('x',i)));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check whether a landmark point exists
|
||||
*/
|
||||
bool landmarkPointExists(int i) const {
|
||||
PointMap::const_iterator it = landmarkPoints_.find(i);
|
||||
return (it != landmarkPoints_.end());
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve landmark point
|
||||
*/
|
||||
Point2 landmarkPoint(int i) const {
|
||||
PointMap::const_iterator it = landmarkPoints_.find(i);
|
||||
if (it == landmarkPoints_.end()) throw(std::invalid_argument(
|
||||
"markerPose: invalid key " + symbol('l',i)));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/**
|
||||
* check whether two configs are equal
|
||||
*/
|
||||
bool equals(const UrbanConfig& c, double tol = 1e-6) const;
|
||||
void addRobotPose(const int i, Pose3 cp);
|
||||
void addLandmark(const int i, Point2 lp);
|
||||
|
||||
void removeRobotPose(const int i);
|
||||
void removeLandmark(const int i);
|
||||
|
||||
void clear() {
|
||||
landmarkPoints_.clear();
|
||||
robotPoses_.clear();
|
||||
}
|
||||
|
||||
inline size_t size() {
|
||||
return landmarkPoints_.size() + robotPoses_.size();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
/*
|
||||
* UrbanFactor.cpp
|
||||
*
|
||||
* Created on: Dec 17, 2009
|
||||
* Author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "UrbanFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
UrbanFactor::UrbanFactor() {
|
||||
// TODO Auto-generated constructor stub
|
||||
}
|
||||
|
||||
UrbanFactor::UrbanFactor(const Vector& z, const double sigma) :
|
||||
NonlinearFactor<UrbanConfig> (z,sigma) {
|
||||
// TODO
|
||||
}
|
||||
|
||||
UrbanFactor::~UrbanFactor() {
|
||||
// TODO Auto-generated destructor stub
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,29 +0,0 @@
|
|||
/*
|
||||
* UrbanFactor.h
|
||||
*
|
||||
* Created on: Dec 17, 2009
|
||||
* Author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#ifndef URBANFACTOR_H_
|
||||
#define URBANFACTOR_H_
|
||||
|
||||
#include "NonlinearFactor.h"
|
||||
#include "UrbanConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Base class for UrbanMeasurement and UrbanOdometry
|
||||
*/
|
||||
class UrbanFactor: public NonlinearFactor<UrbanConfig> {
|
||||
public:
|
||||
|
||||
UrbanFactor();
|
||||
UrbanFactor(const Vector& z, const double sigma);
|
||||
virtual ~UrbanFactor();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* URBANFACTOR_H_ */
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
/**
|
||||
* @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"
|
||||
#include "UrbanMeasurement.h"
|
||||
#include "UrbanOdometry.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(const boost::shared_ptr<const Matrix> &sensorMatrix,
|
||||
double x, double y, double sigma, int i, int j) {
|
||||
Point2 z(x,y);
|
||||
sharedFactor factor(new UrbanMeasurement(sensorMatrix, z,sigma,i,j));
|
||||
push_back(factor);
|
||||
}
|
||||
;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx,
|
||||
double sigmayaw, int p) {
|
||||
Vector z = Vector_(dx,0,0,yaw,0,0);
|
||||
Matrix cov = eye(6);
|
||||
cov(1,1)=sigmadx*sigmadx;
|
||||
cov(4,4)=sigmadx*sigmadx;
|
||||
sharedFactor factor(new UrbanOdometry(symbol('x',p),symbol('x',p+1),z,cov));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanGraph::addOriginConstraint(int p) {
|
||||
// TODO
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
/**
|
||||
* @file UrbanGraph.h
|
||||
* @brief A factor graph for the Urban problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "UrbanFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Non-linear factor graph for visual SLAM
|
||||
*/
|
||||
class UrbanGraph: public gtsam::NonlinearFactorGraph<UrbanConfig> {
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor is empty graph */
|
||||
UrbanGraph();
|
||||
|
||||
/**
|
||||
* print out graph
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const UrbanGraph& p, double tol = 1e-9) const;
|
||||
|
||||
/**
|
||||
* Add a landmark constraint
|
||||
*/
|
||||
void addMeasurement(const boost::shared_ptr<const Matrix> &sensorMatrix,
|
||||
double x, double y, double sigma, int i, int j);
|
||||
|
||||
/**
|
||||
* Add an odometry constraint
|
||||
*/
|
||||
void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int i);
|
||||
|
||||
/**
|
||||
* Add an initial constraint on the first pose
|
||||
*/
|
||||
void addOriginConstraint(int p);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,188 +0,0 @@
|
|||
/**
|
||||
* @file UrbanMeasurement.cpp
|
||||
* @brief A non-linear factor specialized to the Visual SLAM problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include "UrbanMeasurement.h"
|
||||
#include "Pose3.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
#define _GET_TEMPORARIES \
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
// JC: I'd like to break this out into component calculations, but there's too much
|
||||
// Clearly there is a lot of code overlap (and redundant calculations) between
|
||||
// this and the calculation of the relevant jacobians. Need to look at cleaning
|
||||
// this up when I have a better grasp of the larger implications. I don't think
|
||||
// there's any reason we can't just merge transform_to and the two derivative
|
||||
// functions into a single function to remove the overlap.
|
||||
|
||||
|
||||
|
||||
|
||||
UrbanMeasurement::Transform transform_to(const boost::shared_ptr<const Matrix> &sensorMatrix,
|
||||
const Pose3& robotpose, const Point2& lmpos,
|
||||
bool getJacobians) {
|
||||
// Notation:
|
||||
// ph = phi = roll
|
||||
// th = theta = pitch
|
||||
// ps = psi = yaw
|
||||
Vector rpy = RQ(robotpose.rotation().matrix());
|
||||
double dx = lmpos.x() - robotpose.translation().x();
|
||||
double dy = lmpos.y() - robotpose.translation().y();
|
||||
double s23 = (*sensorMatrix)(2, 3);
|
||||
double cps = cos(rpy[2]);
|
||||
double sps = sin(rpy[2]);
|
||||
double secph = 1/cos(rpy[0]);
|
||||
double tph = tan(rpy[0]);
|
||||
double sph = sin(rpy[0]);
|
||||
double sth = sin(rpy[1]);
|
||||
double cth = cos(rpy[1]);
|
||||
double tth = tan(rpy[1]);
|
||||
double secth = 1/cth;
|
||||
|
||||
UrbanMeasurement::Transform ret;
|
||||
ret.get<0>().reset(new Point2(cth*dy*sps
|
||||
+ (-s23*secph+dy*sps*sth+dx*sps*tph)*tth
|
||||
+ cps*(cth*dx+dx*sth*tth-dy*tph*tth),
|
||||
secph*(cps*dy+s23*sph-dx*sps)));
|
||||
|
||||
|
||||
if (getJacobians) {
|
||||
ret.get<1>().reset(new Matrix(2, 6));
|
||||
Matrix &D1 = *(ret.get<1>());
|
||||
D1(0, 0) = (-cps*secth-sps*tph*tth);
|
||||
D1(0, 1) = (-secth*sps+cps*tph*tth);
|
||||
D1(0, 2) = 0;
|
||||
D1(0, 3) = -((secph*secph*(cps*dy+s23*sph-dx*sps)*tth));
|
||||
D1(0, 4) = (-((secth*(secth*(s23*secph+(cps*dy-dx*sps)*tph)
|
||||
+(-cps*dx-dy*sps)*tth))));
|
||||
D1(0, 5) = ((cth*(cps*dy-dx*sps)+(cps*(dy*sth+dx*tph)
|
||||
+sps*(-dx*sth+dy*tph))*tth));
|
||||
|
||||
D1(1, 0) = secph*sps;
|
||||
D1(1, 1) = -cps*secph;
|
||||
D1(1, 2) = 0;
|
||||
D1(1, 3) = secph*(s23*secph+(cps*dy-dx*sps)*tph);
|
||||
D1(1, 4) = 0;
|
||||
D1(1, 5) = secph*(-cps*dx-dy*sps);
|
||||
|
||||
|
||||
ret.get<2>().reset(new Matrix(2, 2));
|
||||
Matrix &D2 = *(ret.get<2>());
|
||||
D2(0, 0) = cps*secth+sps*tph*tth;
|
||||
D2(0, 1) = secth*sps-cps*tph*tth;
|
||||
D2(1, 0) = -secph*sps;
|
||||
D2(1, 1) = cps*secph;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
UrbanMeasurement::UrbanMeasurement() {
|
||||
/// Arbitrary values
|
||||
|
||||
// JC: This should cause use of a default-constructed measurement to segfault.
|
||||
// Not entirely sure as to the desired behaviour when uninitialized data is used.
|
||||
// If you're crashing because of this and you didn't expect to, sorry. :)
|
||||
sensorMatrix_.reset();
|
||||
|
||||
robotPoseNumber_ = 111;
|
||||
landmarkNumber_ = 222;
|
||||
robotPoseName_ = symbol('x', robotPoseNumber_);
|
||||
landmarkName_ = symbol('l', landmarkNumber_);
|
||||
keys_.push_back(robotPoseName_);
|
||||
keys_.push_back(landmarkName_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
UrbanMeasurement::UrbanMeasurement(const boost::shared_ptr<const Matrix> &sensorMatrix,
|
||||
const Point2& z, double sigma, int i,
|
||||
int j) :
|
||||
UrbanFactor(z.vector(), sigma) {
|
||||
|
||||
sensorMatrix_ = sensorMatrix;
|
||||
robotPoseNumber_ = i;
|
||||
landmarkNumber_ = j;
|
||||
robotPoseName_ = symbol('x', robotPoseNumber_);
|
||||
landmarkName_ = symbol('l', landmarkNumber_);
|
||||
keys_.push_back(robotPoseName_);
|
||||
keys_.push_back(landmarkName_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanMeasurement::print(const std::string& s) const {
|
||||
printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(),
|
||||
landmarkName_.c_str());
|
||||
gtsam::print(this->z_, s + ".z");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool UrbanMeasurement::equals(const UrbanMeasurement& p, double tol) const {
|
||||
if (&p == NULL) return false;
|
||||
if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_
|
||||
!= p.landmarkNumber_) return false;
|
||||
if (!equal_with_abs_tol(this->z_, p.z_, tol)) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector UrbanMeasurement::error_vector(const UrbanConfig& x) const {
|
||||
Pose3 pose = x.robotPose(robotPoseNumber_);
|
||||
Point2 landmark = x.landmarkPoint(landmarkNumber_);
|
||||
// Right-hand-side b = (z - h(x))/sigma
|
||||
// fprintf(stderr, "p: %.16g l: %.16g\n",
|
||||
// pose.rotation().vector()[0],
|
||||
// landmark.x());
|
||||
boost::shared_ptr<Point2> h = transform_to(sensorMatrix_, pose,landmark, false).get<0>();
|
||||
// fprintf(stderr, "H: %.16g %.16g\nz: %.16g %.16g\n",
|
||||
// h->vector()[0], h->vector()[1],
|
||||
// z_[0], z_[1]);
|
||||
return (z_ - h->vector());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactor::shared_ptr UrbanMeasurement::linearize(const UrbanConfig& config) const {
|
||||
Pose3 pose = config.robotPose(robotPoseNumber_);
|
||||
Point2 landmark = config.landmarkPoint(landmarkNumber_);
|
||||
|
||||
// JC: Things I don't yet understand:
|
||||
//
|
||||
// What should the keys be? Presumably I just need to match a convention.
|
||||
//
|
||||
// Should the jacobians be premultiplied at the time of
|
||||
// GaussianFactor construction?
|
||||
//
|
||||
// Is GaussianFactor copying its constructor arguments? (If not, this is
|
||||
// not safe, nor is what this code replaced).
|
||||
//
|
||||
// Why is sigma a double instead of a matrix?
|
||||
|
||||
|
||||
ostringstream k;
|
||||
k << "pose" << robotPoseNumber_;
|
||||
string posekey = k.str();
|
||||
k.clear();
|
||||
k << "lm" << landmarkNumber_;
|
||||
string lmkey = k.str();
|
||||
k.clear();
|
||||
|
||||
Transform tr = transform_to(sensorMatrix_, pose,landmark, true);
|
||||
|
||||
|
||||
return GaussianFactor::shared_ptr(
|
||||
new GaussianFactor(posekey,
|
||||
*(tr.get<1>()),
|
||||
lmkey,
|
||||
*(tr.get<2>()),
|
||||
z_ - tr.get<0>()->vector(),
|
||||
sigma_));
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,104 +0,0 @@
|
|||
/**
|
||||
* @file UrbanMeasurement.h
|
||||
* @brief A Nonlinear Factor, specialized for Urban application
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "UrbanFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class UrbanConfig;
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement,
|
||||
*/
|
||||
class UrbanMeasurement: public UrbanFactor {
|
||||
private:
|
||||
boost::shared_ptr<const Matrix> sensorMatrix_;
|
||||
int robotPoseNumber_, landmarkNumber_;
|
||||
std::string robotPoseName_, landmarkName_;
|
||||
//boost::shared_ptr<Cal3_S2> K_; // Calibration stored in each factor. FD: need to think about this.
|
||||
typedef NonlinearFactor<UrbanConfig> ConvenientFactor;
|
||||
|
||||
|
||||
public:
|
||||
// Return type for the transform_to function
|
||||
typedef boost::tuple<boost::shared_ptr<Point2>,
|
||||
boost::shared_ptr<Matrix>,
|
||||
boost::shared_ptr<Matrix> > Transform;
|
||||
|
||||
typedef boost::shared_ptr<UrbanMeasurement> shared_ptr; // shorthand for a smart pointer to a factor
|
||||
//typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
|
||||
|
||||
/**
|
||||
* Default constructor
|
||||
*/
|
||||
UrbanMeasurement();
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param z is the 2 dimensional location of landmark respect to the robot (the measurement)
|
||||
* @param sigma is the standard deviation
|
||||
* @param robotPoseNumber is basically the pose number
|
||||
* @param landmarkNumber is the index of the landmark
|
||||
*
|
||||
*
|
||||
*/
|
||||
UrbanMeasurement(const boost::shared_ptr<const Matrix> &sensorMatrix,
|
||||
const Point2& z, double sigma, int robotPoseNumber,
|
||||
int landmarkNumber);
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
*/
|
||||
void print(const std::string& s = "UrbanMeasurement") const;
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const UrbanMeasurement&, double tol = 1e-9) const;
|
||||
|
||||
/**
|
||||
* calculate the error of the factor
|
||||
*/
|
||||
Vector error_vector(const UrbanConfig&) const;
|
||||
|
||||
/**
|
||||
* linerarization
|
||||
*/
|
||||
|
||||
GaussianFactor::shared_ptr linearize(const UrbanConfig&) const;
|
||||
|
||||
int getrobotPoseNumber() const {
|
||||
return robotPoseNumber_;
|
||||
}
|
||||
int getLandmarkNumber() const {
|
||||
return landmarkNumber_;
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(robotPoseNumber_);
|
||||
ar & BOOST_SERIALIZATION_NVP(landmarkNumber_);
|
||||
ar & BOOST_SERIALIZATION_NVP(robotPoseName_);
|
||||
ar & BOOST_SERIALIZATION_NVP(landmarkName_);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Transform 2D landmark into 6D pose, and its derivatives
|
||||
*/
|
||||
// JC: These are exported only for testbenches?
|
||||
UrbanMeasurement::Transform transform_to(const boost::shared_ptr<const Matrix> &sensorMatrix,
|
||||
const Pose3& robotpose, const Point2& lmpos,
|
||||
bool getJacobians);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,86 +0,0 @@
|
|||
/**
|
||||
* @file UrbanOdometry.cpp
|
||||
* @brief A non-linear factor specialized to the Visual SLAM problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include "UrbanOdometry.h"
|
||||
|
||||
using namespace std;
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* *
|
||||
UrbanOdometry::UrbanOdometry() {
|
||||
/// Arbitrary values
|
||||
robotPoseNumber_ = 1;
|
||||
robotPoseName_ = symbol('x',robotPoseNumber_);
|
||||
keys_.push_back(robotPoseName_);
|
||||
}
|
||||
/* ************************************************************************* *
|
||||
UrbanOdometry::UrbanOdometry(const Point2& z, double sigma, int cn, int ln)
|
||||
: NonlinearFactor<Pose3Config>(z.vector(), sigma)
|
||||
{
|
||||
robotPoseNumber_ = cn;
|
||||
landmarkNumber_ = ln;
|
||||
robotPoseName_ = symbol('x',robotPoseNumber_);
|
||||
landmarkName_ = symbol('l',landmarkNumber_);
|
||||
keys_.push_back(robotPoseName_);
|
||||
keys_.push_back(landmarkName_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
void UrbanOdometry::print(const std::string& s) const {
|
||||
printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str());
|
||||
gtsam::print(this->z_, s+".z");
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
bool UrbanOdometry::equals(const UrbanOdometry& p, double tol) const {
|
||||
if (&p == NULL) return false;
|
||||
if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
|
||||
if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
// TODO Implement transformPoint2_from
|
||||
// the difference here that we have a 2d point in a 3D coordinate
|
||||
Vector UrbanOdometry::predict(const Pose3Config& c) const {
|
||||
Pose3 pose = c.cameraPose(robotPoseNumber_);
|
||||
Point2 landmark = c.landmarkPoint(landmarkNumber_);
|
||||
return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector();
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
Vector UrbanOdometry::error_vector(const Pose3Config& c) const {
|
||||
// Right-hand-side b = (z - h(x))/sigma
|
||||
Point2 h = predict(c);
|
||||
return (this->z_ - h);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
GaussianFactor::shared_ptr UrbanOdometry::linearize(const Pose3Config& c) const
|
||||
{
|
||||
// get arguments from config
|
||||
Pose3 pose = c.cameraPose(robotPoseNumber_);
|
||||
Point3 landmark = c.landmarkPoint(landmarkNumber_);
|
||||
|
||||
SimpleCamera camera(*K_,pose);
|
||||
|
||||
// Jacobians
|
||||
Matrix Dh1 = Dproject_pose(camera, landmark);
|
||||
Matrix Dh2 = Dproject_point(camera, landmark);
|
||||
|
||||
// Right-hand-side b = (z - h(x))
|
||||
Vector h = project(camera, landmark).vector();
|
||||
Vector b = this->z_ - h;
|
||||
|
||||
// Make new linearfactor, divide by sigma
|
||||
GaussianFactor::shared_ptr
|
||||
p(new GaussianFactor(robotPoseName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
|
||||
return p;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,74 +0,0 @@
|
|||
/**
|
||||
* @file UrbanOdometry.h
|
||||
* @brief A Nonlinear Factor, specialized for Urban application
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "UrbanFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class UrbanOdometry: public UrbanFactor {
|
||||
private:
|
||||
std::string key1_, key2_; /** The keys of the two poses, order matters */
|
||||
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<UrbanOdometry> shared_ptr; // shorthand for a smart pointer to a factor
|
||||
|
||||
UrbanOdometry(const std::string& key1, const std::string& key2,
|
||||
const Vector& measured, const Matrix& measurement_covariance) :
|
||||
UrbanFactor(measured, 1.0),
|
||||
key1_(key1), key2_(key2) {
|
||||
square_root_inverse_covariance_ = inverse_square_root(
|
||||
measurement_covariance);
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
void print(const std::string& name) const {
|
||||
std::cout << name << std::endl;
|
||||
std::cout << "Factor " << std::endl;
|
||||
std::cout << "key1 " << key1_ << std::endl;
|
||||
std::cout << "key2 " << key2_ << std::endl;
|
||||
gtsam::print(z_,"measured ");
|
||||
gtsam::print(square_root_inverse_covariance_, "square_root_inverse_covariance");
|
||||
}
|
||||
|
||||
bool equals(const NonlinearFactor<UrbanConfig>& expected, double tol) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
Vector error_vector(const UrbanConfig& config) const {
|
||||
return zero(6);
|
||||
}
|
||||
|
||||
std::list<std::string> keys() const {
|
||||
std::list<std::string> l;
|
||||
l.push_back(key1_);
|
||||
l.push_back(key2_);
|
||||
return l;
|
||||
}
|
||||
|
||||
std::size_t size() const {
|
||||
return 2;
|
||||
}
|
||||
|
||||
/** linearize */
|
||||
boost::shared_ptr<GaussianFactor> linearize(const UrbanConfig& config) const {
|
||||
Vector b = zero(6);
|
||||
Matrix H1 = zeros(6,6);
|
||||
Matrix H2 = zeros(6,6);
|
||||
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_,
|
||||
square_root_inverse_covariance_ * H1, key2_,
|
||||
square_root_inverse_covariance_ * H2, b, 1.0));
|
||||
return linearized;
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
/*
|
||||
* @file testUrbanConfig.cpp
|
||||
* @brief Tests for the CitySLAM configuration class
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <UrbanConfig.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( UrbanConfig, update_with_large_delta)
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,156 +0,0 @@
|
|||
/**
|
||||
* @file testUrbanGraph.cpp
|
||||
* @brief Unit test for two robot poses and four landmarks
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "UrbanGraph.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "Ordering.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
typedef NonlinearOptimizer<UrbanGraph,UrbanConfig> Optimizer;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
static Point2 landmark( 2, 5);
|
||||
static Point2 landmark2( 2, 10);
|
||||
static Point2 landmark3( -2, 5);
|
||||
static Point2 landmark4( -2,-10);
|
||||
|
||||
/* Robot is at (0,0,0) looking in global "y" direction.
|
||||
* For the local frame we used Navlab convention,
|
||||
* x-looks forward, y-looks right, z- down*/
|
||||
static Pose3 robotPose(Matrix_(3,3,
|
||||
0., 1., 0.,
|
||||
1., 0., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,0,0));
|
||||
|
||||
/* move at a speed 10 m/s (36 km/h or 22 mph), 10 Hz update, we move 1m.*/
|
||||
static Pose3 robotPose2(Matrix_(3,3,
|
||||
0., 1., 0.,
|
||||
1., 0., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,1,0));
|
||||
|
||||
static boost::shared_ptr<const Matrix> sensorMatrix(new Matrix(4, 4));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( UrbanGraph, addMeasurement)
|
||||
{
|
||||
// Check adding a measurement
|
||||
UrbanGraph g;
|
||||
double sigma = 0.2;// 20 cm
|
||||
g.addMeasurement(sensorMatrix, 4, 2, sigma, 1, 1); // ground truth would be (5,2)
|
||||
LONGS_EQUAL(1,g.size());
|
||||
|
||||
// Create a config
|
||||
UrbanConfig config;
|
||||
config.addRobotPose(1,robotPose);
|
||||
config.addLandmark(1,landmark);
|
||||
|
||||
// Check error
|
||||
double expected = 0.5/sigma/sigma;
|
||||
DOUBLES_EQUAL(expected, g.error(config), 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( UrbanGraph, addOdometry)
|
||||
{
|
||||
// Check adding a measurement
|
||||
UrbanGraph g;
|
||||
double sigmadx = 0.01; // 1 cm
|
||||
double sigmayaw = M_PI/180.0;// 1 degree
|
||||
g.addOdometry(2, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw
|
||||
LONGS_EQUAL(1,g.size());
|
||||
|
||||
// Create a config
|
||||
UrbanConfig config;
|
||||
config.addRobotPose(1,robotPose);
|
||||
config.addLandmark(1,landmark);
|
||||
|
||||
// Check error
|
||||
double expected = 0.5/0.01/0.01;
|
||||
// TODO DOUBLES_EQUAL(expected, g.error(config), 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
* the measurements are relative to the robot pose so
|
||||
* they are in robot coordinate frame different from the global (above)
|
||||
*/
|
||||
UrbanGraph testGraph() {
|
||||
|
||||
double sigma = 0.2;// 20 cm
|
||||
double sigmadx = 0.01; // 1 cm
|
||||
double sigmayaw = M_PI/180.0;// 1 degree
|
||||
|
||||
UrbanGraph g;
|
||||
g.addOriginConstraint(1); // pose1 is the origin
|
||||
g.addMeasurement(sensorMatrix, 5, 2, sigma, 1, 1); // z11
|
||||
g.addMeasurement(sensorMatrix, 10, 2, sigma, 1, 2); // z12
|
||||
g.addMeasurement(sensorMatrix, 5, -2, sigma, 1, 3); // z13
|
||||
g.addMeasurement(sensorMatrix, 10, -2, sigma, 1, 4); // z14
|
||||
g.addOdometry(1, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw
|
||||
g.addMeasurement(sensorMatrix, 4, 2, sigma, 2, 1); // z21
|
||||
g.addMeasurement(sensorMatrix, 9, 2, sigma, 2, 2); // z22
|
||||
g.addMeasurement(sensorMatrix, 4, -2, sigma, 2, 3); // z23
|
||||
g.addMeasurement(sensorMatrix, 9, -2, sigma, 2, 4); // z24
|
||||
|
||||
return g;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( UrbanGraph, optimizeLM)
|
||||
{
|
||||
// build a graph
|
||||
UrbanGraph graph = testGraph();
|
||||
|
||||
// Create an initial configuration corresponding to the ground truth
|
||||
boost::shared_ptr<UrbanConfig> initialEstimate(new UrbanConfig);
|
||||
initialEstimate->addRobotPose(1, robotPose);
|
||||
initialEstimate->addRobotPose(2, robotPose2);
|
||||
initialEstimate->addLandmark(1, landmark);
|
||||
initialEstimate->addLandmark(2, landmark2);
|
||||
initialEstimate->addLandmark(3, landmark3);
|
||||
initialEstimate->addLandmark(4, landmark4);
|
||||
|
||||
// Check error vector
|
||||
Vector expected = zero(16);
|
||||
// TODO CHECK(assert_equal(expected, graph.error_vector(*initialEstimate)));
|
||||
|
||||
// Check error = zero with this config
|
||||
//DOUBLES_EQUAL(55, graph.error(*initialEstimate), 1e-9);
|
||||
|
||||
// Create an ordering of the variables
|
||||
Ordering ordering;
|
||||
ordering += "l1","l2","l3","l4","x1","x2"; // boost assign syntax
|
||||
|
||||
// Create an optimizer and check its error
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
|
||||
// TODO DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
// with the ground truth
|
||||
|
||||
// Optimizer afterOneIteration = optimizer.iterate();
|
||||
// TODO DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
|
||||
|
||||
// check if correct
|
||||
// TODO CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,120 +0,0 @@
|
|||
/**********************************************************
|
||||
Written by Frank Dellaert, Dec 2009
|
||||
**********************************************************/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "UrbanConfig.h"
|
||||
#include "UrbanMeasurement.h"
|
||||
#include "Vector.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
||||
/*
|
||||
* This unit test cross checks UrbanMeasurement with the
|
||||
* results from Justin's thesis codebase.
|
||||
*/
|
||||
|
||||
static boost::shared_ptr<Matrix> sensorMatrix(new Matrix(4, 4));
|
||||
|
||||
static bool _data_inited = false;
|
||||
|
||||
static void _init() {
|
||||
// Not MT-safe. Don't think that's an issue for CppUnitLite
|
||||
if (_data_inited) {
|
||||
return;
|
||||
}
|
||||
(*sensorMatrix)(0, 0) = 0.0251;
|
||||
(*sensorMatrix)(0, 1) = 0.9996;
|
||||
(*sensorMatrix)(0, 2) = 0.0091;
|
||||
(*sensorMatrix)(0, 3) = 1.5574;
|
||||
(*sensorMatrix)(1, 0) = -0.9996;
|
||||
(*sensorMatrix)(1, 1) = 0.0253;
|
||||
(*sensorMatrix)(1, 2) = -0.016;
|
||||
(*sensorMatrix)(1, 3) = -0.86;
|
||||
(*sensorMatrix)(2, 0) = -0.0163;
|
||||
(*sensorMatrix)(2, 1) = -0.0087;
|
||||
(*sensorMatrix)(2, 2) = 0.9998;
|
||||
(*sensorMatrix)(2, 3) = -0.6478;
|
||||
(*sensorMatrix)(3, 0) = 0;
|
||||
(*sensorMatrix)(3, 1) = 0;
|
||||
(*sensorMatrix)(3, 2) = 0;
|
||||
(*sensorMatrix)(3, 3) = 1;
|
||||
_data_inited = true;
|
||||
|
||||
}
|
||||
/* Robot is at (0,0,0) looking in global "y" direction.
|
||||
* For the local frame we used Navlab convention,
|
||||
* x-looks forward, y-looks right, z- down*/
|
||||
static const Pose3 robotPose(Matrix_(4, 4,
|
||||
-0.0848808908061426, 0.993833753552749900, 0.0713421661796702400, 377.7509309858,
|
||||
-0.9963883079644135, -0.08449313834814816, -0.008440931458908668, 1922.5523404841,
|
||||
-0.002360959078213238, -0.07180097402774338, 0.9974161849503438, -6369417.591562273,
|
||||
0.0, 0.0, 0.0, 1));
|
||||
|
||||
static const Point2 landmark(362.707949418, 1936.2843137418);
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( UrbanMeasurement, transform_to )
|
||||
{
|
||||
_init();
|
||||
UrbanMeasurement::Transform tr = transform_to(sensorMatrix, robotPose, landmark, true);
|
||||
|
||||
// Check the transformation from global to sensor coordinates
|
||||
CHECK(assert_equal(Point2(-12.40679724375481, -16.14944758846734),*(tr.get<0>())));
|
||||
|
||||
Point2 &foo = *(tr.get<0>());
|
||||
|
||||
// Check the jacobian w.r.t. the pose
|
||||
CHECK(assert_equal(Matrix_(2, 6,
|
||||
0.08471201853652961, 0.9964082882836194, 0.0, 0.03822695627705205, -0.5427133154140961, -16.15221468780303,
|
||||
-0.998969460290058, 0.0851007754705408, 0.0, 0.5147498807397776, 0.0, 12.43765251715327),
|
||||
*(tr.get<1>())));
|
||||
|
||||
// Check the jacobian w.r.t the landmark
|
||||
CHECK(assert_equal(Matrix_(2, 2,
|
||||
-0.08471201853652961, -0.9964082882836194,
|
||||
0.998969460290058, -0.0851007754705408),
|
||||
*(tr.get<2>())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( UrbanMeasurement, error_vector )
|
||||
{
|
||||
_init();
|
||||
|
||||
Point2 z(-12.42876033350471, -16.10191021850896);
|
||||
double sigma = 0.2;
|
||||
UrbanMeasurement factor(sensorMatrix, z,sigma,44,66);
|
||||
|
||||
// Create a config
|
||||
UrbanConfig config;
|
||||
config.addRobotPose(44,robotPose);
|
||||
config.addLandmark(66,landmark);
|
||||
|
||||
// test error_vector
|
||||
Vector expected = Vector_(2,
|
||||
-.02196308974990, .04753736995838);
|
||||
Vector actual = factor.error_vector(config);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
// test error. Expected was generated via manual calculation from
|
||||
// error vector.
|
||||
double expected2 = .03427723560;
|
||||
double actual2 = factor.error(config);
|
||||
DOUBLES_EQUAL(expected2,actual2,1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
TestRegistry::runAllTests(tr);
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
/**
|
||||
* @file testUrbanOdometry.cpp
|
||||
* @brief Unit tests for UrbanOdometry Class
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
/*STL/C++*/
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "UrbanOdometry.h"
|
||||
#include "Pose2Graph.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
||||
TEST( UrbanOdometry, constructor )
|
||||
{
|
||||
string key1 = "x1", key2 = "x2";
|
||||
Vector measured = zero(6);
|
||||
Matrix measurement_covariance = eye(6);
|
||||
UrbanOdometry factor(key1, key2, measured, measurement_covariance);
|
||||
list<string> expected;
|
||||
expected += "x1","x2";
|
||||
CHECK(factor.keys()==expected);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue