UrbanGraph class and testUrbanGraph TODOs
Viorela Ila
parent
68de3ae715
commit
d61d92c0e7
|
|
@ -127,9 +127,11 @@ headers += SQPOptimizer.h SQPOptimizer-inl.h
|
||||||
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
|
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
|
||||||
headers += NonlinearEquality.h
|
headers += NonlinearEquality.h
|
||||||
headers += Pose2Config.h Pose2Factor.h
|
headers += Pose2Config.h Pose2Factor.h
|
||||||
|
headers += UrbanGraph.h
|
||||||
sources += NonlinearFactor.cpp
|
sources += NonlinearFactor.cpp
|
||||||
sources += Pose2Graph.cpp
|
sources += Pose2Graph.cpp
|
||||||
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph
|
sources += UrbanConfig.cpp UrbanMeasurement.cpp
|
||||||
|
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph testUrbanGraph
|
||||||
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
|
||||||
testNonlinearFactor_LDADD = libgtsam.la
|
testNonlinearFactor_LDADD = libgtsam.la
|
||||||
testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp
|
testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp
|
||||||
|
|
@ -148,6 +150,8 @@ testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
|
||||||
testPose2Factor_LDADD = libgtsam.la
|
testPose2Factor_LDADD = libgtsam.la
|
||||||
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
|
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
|
||||||
testPose2Graph_LDADD = libgtsam.la
|
testPose2Graph_LDADD = libgtsam.la
|
||||||
|
testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp
|
||||||
|
testUrbanGraph_LDADD = libgtsam.la
|
||||||
|
|
||||||
# geometry
|
# geometry
|
||||||
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
|
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
|
||||||
|
|
|
||||||
|
|
@ -101,25 +101,44 @@ Vector hPose (const Vector& x) {
|
||||||
*/
|
*/
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Matrix DhPose(const Vector& x) {
|
Matrix DhPose(const Vector& x) {
|
||||||
Matrix H = eye(6,6);
|
Matrix H = eye(6,6);
|
||||||
return H;
|
return H;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Pose3 Pose3::inverse() const
|
Pose3 Pose3::inverse() const
|
||||||
{
|
{
|
||||||
Rot3 Rt = R_.inverse();
|
Rot3 Rt = R_.inverse();
|
||||||
return Pose3(Rt,-(Rt*t_));
|
return Pose3(Rt,-(Rt*t_));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Pose3 Pose3::transformPose_to(const Pose3& pose) const
|
Pose3 Pose3::transformPose_to(const Pose3& pose) const
|
||||||
{
|
{
|
||||||
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
|
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
|
||||||
Point3 t = transform_to(pose, t_);
|
Point3 t = transform_to(pose, t_);
|
||||||
|
|
||||||
return Pose3(cRv, t);
|
return Pose3(cRv, t);
|
||||||
}
|
}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
|
||||||
|
Pose3 between(const Pose3& p1, const Pose3& p2){
|
||||||
|
Pose3 p;
|
||||||
|
return p;
|
||||||
|
// TODO: implement
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix Dbetween1(const Pose3& p1, const Pose3& p2){
|
||||||
|
Matrix m;
|
||||||
|
return m;
|
||||||
|
// TODO: implement
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix Dbetween2(const Pose3& p1, const Pose3& p2){
|
||||||
|
Matrix m;
|
||||||
|
return m;
|
||||||
|
// TODO: implement
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
20
cpp/Pose3.h
20
cpp/Pose3.h
|
|
@ -77,6 +77,19 @@ namespace gtsam {
|
||||||
/** inverse transformation */
|
/** inverse transformation */
|
||||||
Pose3 inverse() const;
|
Pose3 inverse() const;
|
||||||
|
|
||||||
|
// operators
|
||||||
|
Pose3 operator+(const Pose3& p3) const{
|
||||||
|
// TODO implement the operators "+"
|
||||||
|
Pose3 p;
|
||||||
|
return p;
|
||||||
|
};
|
||||||
|
|
||||||
|
Pose3 operator-(const Pose3& p3) const{
|
||||||
|
// TODO implement the operators "-"
|
||||||
|
Pose3 p;
|
||||||
|
return p;
|
||||||
|
};
|
||||||
|
|
||||||
/** composition */
|
/** composition */
|
||||||
inline Pose3 operator*(const Pose3& B) const {
|
inline Pose3 operator*(const Pose3& B) const {
|
||||||
return Pose3(R_*B.R_, t_ + R_*B.t_);
|
return Pose3(R_*B.R_, t_ + R_*B.t_);
|
||||||
|
|
@ -123,6 +136,13 @@ namespace gtsam {
|
||||||
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
|
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
|
||||||
Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !
|
Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||||
|
*/
|
||||||
|
Pose3 between(const Pose3& p1, const Pose3& p2);
|
||||||
|
Matrix Dbetween1(const Pose3& p1, const Pose3& p2);
|
||||||
|
Matrix Dbetween2(const Pose3& p1, const Pose3& p2);
|
||||||
|
|
||||||
/** direct measurement of a pose */
|
/** direct measurement of a pose */
|
||||||
Vector hPose(const Vector& x);
|
Vector hPose(const Vector& x);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,87 @@
|
||||||
|
/**
|
||||||
|
* @file UrbanOdometryFactor.cpp
|
||||||
|
* @brief A non-linear factor specialized to the Visual SLAM problem
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Viorela Ila
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "UrbanConfig.h"
|
||||||
|
#include "Pose3Factor.h"
|
||||||
|
#include "Pose3.h"
|
||||||
|
#include "SimpleCamera.h"
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
namespace gtsam{
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Pose3Factor::Pose3Factor() {
|
||||||
|
/// Arbitrary values
|
||||||
|
robotPoseNumber_ = 1;
|
||||||
|
robotPoseName_ = symbol('x',robotPoseNumber_);
|
||||||
|
keys_.push_back(robotPoseName_);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Pose3Factor::Pose3Factor(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 Pose3Factor::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 Pose3Factor::equals(const Pose3Factor& 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 Pose3Factor::predict(const Pose3Config& c) const {
|
||||||
|
Pose3 pose = c.cameraPose(robotPoseNumber_);
|
||||||
|
Point2 landmark = c.landmarkPoint(landmarkNumber_);
|
||||||
|
return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector();
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector Pose3Factor::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 Pose3Factor::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
|
||||||
|
|
@ -0,0 +1,71 @@
|
||||||
|
/**
|
||||||
|
* @file Pose3Factor.h
|
||||||
|
* @brief A Nonlinear Factor, specialized for Urban application
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Viorela Ila
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <map>
|
||||||
|
#include "NonlinearFactor.h"
|
||||||
|
#include "GaussianFactor.h"
|
||||||
|
#include "VectorConfig.h"
|
||||||
|
#include "Pose3.h"
|
||||||
|
#include "Matrix.h"
|
||||||
|
#include "ostream"
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
template <class Config>
|
||||||
|
class Pose3Factor : public NonlinearFactor<Config> {
|
||||||
|
private:
|
||||||
|
std::string key1_, key2_; /** The keys of the two poses, order matters */
|
||||||
|
Pose3 measured_;
|
||||||
|
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
typedef boost::shared_ptr<Pose3Factor> shared_ptr; // shorthand for a smart pointer to a factor
|
||||||
|
|
||||||
|
Pose3Factor(const std::string& key1, const std::string& key2,
|
||||||
|
const Pose3& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) {
|
||||||
|
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;
|
||||||
|
measured_.print("measured ");
|
||||||
|
gtsam::print(square_root_inverse_covariance_,"MeasurementCovariance");
|
||||||
|
}
|
||||||
|
bool equals(const NonlinearFactor<Config>& expected, double tol) const {return false;}
|
||||||
|
|
||||||
|
/** implement functions needed to derive from Factor */
|
||||||
|
Vector error_vector(const Config& config) const {
|
||||||
|
//z-h
|
||||||
|
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
|
||||||
|
return (measured_ - between(p1,p2)).vector();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::list<std::string> keys() const { std::list<std::string> l; return l; }
|
||||||
|
std::size_t size() const { return 2;}
|
||||||
|
|
||||||
|
/** linearize */
|
||||||
|
boost::shared_ptr<GaussianFactor> linearize(const Config& config) const {
|
||||||
|
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
|
||||||
|
Vector b = (measured_ - between(p1,p2)).vector();
|
||||||
|
Matrix H1 = Dbetween1(p1,p2);
|
||||||
|
Matrix H2 = Dbetween2(p1,p2);
|
||||||
|
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
|
||||||
|
|
||||||
|
|
@ -0,0 +1,121 @@
|
||||||
|
/**
|
||||||
|
* @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)
|
||||||
|
{
|
||||||
|
pair<int, Pose3> robot;
|
||||||
|
robot.first = i;
|
||||||
|
robot.second = cp;
|
||||||
|
robotPoses_.insert(robot);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void UrbanConfig::addLandmark(const int i, Point2 lp)
|
||||||
|
{
|
||||||
|
pair<int, Point2> landmark;
|
||||||
|
landmark.first = i;
|
||||||
|
landmark.second = lp;
|
||||||
|
landmarkPoints_.insert(landmark);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
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
|
||||||
|
|
||||||
|
|
@ -0,0 +1,121 @@
|
||||||
|
/**
|
||||||
|
* @file UrbanConfig.h
|
||||||
|
* @brief Config for Urban application
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Viorela Ila
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <map>
|
||||||
|
#include <vector>
|
||||||
|
#include <fstream>
|
||||||
|
#include "VectorConfig.h"
|
||||||
|
#include "Pose3.h"
|
||||||
|
#include "Point2.h"
|
||||||
|
#include "Testable.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_;
|
||||||
|
|
||||||
|
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):
|
||||||
|
robotPoses_(original.robotPoses_), landmarkPoints_(original.landmarkPoints_){}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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);
|
||||||
|
if (it==robotPoses_.end())
|
||||||
|
return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
Pose3 robotPose(int i) const {
|
||||||
|
PoseMap::const_iterator it = robotPoses_.find(i);
|
||||||
|
if (it==robotPoses_.end())
|
||||||
|
throw(std::invalid_argument("robotPose: invalid key"));
|
||||||
|
return it->second;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Check whether a landmark point exists
|
||||||
|
*/
|
||||||
|
bool landmarkPointExists(int i) const
|
||||||
|
{
|
||||||
|
PointMap::const_iterator it = landmarkPoints_.find(i);
|
||||||
|
if (it==landmarkPoints_.end())
|
||||||
|
return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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"));
|
||||||
|
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
|
||||||
|
|
||||||
|
|
@ -0,0 +1,69 @@
|
||||||
|
/**
|
||||||
|
* @file UrbanGraph.h
|
||||||
|
* @brief A factor graph for the Urban problem
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Viorela Ila
|
||||||
|
*/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
#include <map>
|
||||||
|
#include <set>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
#include "NonlinearFactorGraph.h"
|
||||||
|
#include "FactorGraph-inl.h"
|
||||||
|
#include "UrbanMeasurement.h"
|
||||||
|
#include "Pose3Factor.h"
|
||||||
|
#include "UrbanConfig.h"
|
||||||
|
#include "Testable.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 {
|
||||||
|
gtsam::NonlinearFactorGraph<UrbanConfig>::print(s);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* equals
|
||||||
|
*/
|
||||||
|
bool equals(const UrbanGraph& p, double tol=1e-9) const {
|
||||||
|
return gtsam::NonlinearFactorGraph<UrbanConfig>::equals(p, tol);
|
||||||
|
}
|
||||||
|
// TODO implement addMeasurenment, addOdometry and addOriginalConstraint (out of the class)
|
||||||
|
/**
|
||||||
|
* Add a landmark constraint
|
||||||
|
*/
|
||||||
|
void addMeasurement(double x, double y, double sigma, int p1, int p2) {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Add an odometry constraint
|
||||||
|
*/
|
||||||
|
void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int p ) {};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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
|
||||||
|
|
@ -0,0 +1,76 @@
|
||||||
|
/**
|
||||||
|
* @file UrbanMeasurement.cpp
|
||||||
|
* @brief A non-linear factor specialized to the Visual SLAM problem
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Viorela Ila
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "UrbanConfig.h"
|
||||||
|
#include "UrbanMeasurement.h"
|
||||||
|
#include "Pose3.h"
|
||||||
|
#include "SimpleCamera.h"
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
namespace gtsam{
|
||||||
|
/** receives the 2D point in world coordinates and transforms it to Pose coordinates */
|
||||||
|
Point3 transformPoint2_to(const Pose3& pose, const Point2& p);
|
||||||
|
Matrix DtransformPoint2_to1(const Pose3& pose, const Point2& p);
|
||||||
|
Matrix DtransformPoint2_to2(const Pose3& pose); // does not depend on p !
|
||||||
|
/* ************************************************************************* */
|
||||||
|
UrbanMeasurement::UrbanMeasurement() {
|
||||||
|
/// Arbitrary values
|
||||||
|
robotPoseNumber_ = 111;
|
||||||
|
landmarkNumber_ = 222;
|
||||||
|
robotPoseName_ = symbol('x',robotPoseNumber_);
|
||||||
|
landmarkName_ = symbol('l',landmarkNumber_);
|
||||||
|
keys_.push_back(robotPoseName_);
|
||||||
|
keys_.push_back(landmarkName_);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
UrbanMeasurement::UrbanMeasurement(const Point2& z, double sigma, int cn, int ln)
|
||||||
|
: NonlinearFactor<UrbanConfig>(z.vector(), sigma)
|
||||||
|
{
|
||||||
|
robotPoseNumber_ = cn;
|
||||||
|
landmarkNumber_ = ln;
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// the difference here that we have a 2d point in a 3D coordinate
|
||||||
|
Vector UrbanMeasurement::predict(const UrbanConfig& c) const {
|
||||||
|
Pose3 pose = c.robotPose(robotPoseNumber_);
|
||||||
|
Point2 landmark = c.landmarkPoint(landmarkNumber_);
|
||||||
|
// TODO Implement predict function
|
||||||
|
Vector v;
|
||||||
|
return v;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector UrbanMeasurement::error_vector(const UrbanConfig& c) const {
|
||||||
|
// Right-hand-side b = (z - h(x))/sigma
|
||||||
|
Point2 h = predict(c);
|
||||||
|
// TODO Return z-h(x)
|
||||||
|
Vector v;
|
||||||
|
return v;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,100 @@
|
||||||
|
/**
|
||||||
|
* @file UrbanMeasurement.h
|
||||||
|
* @brief A Nonlinear Factor, specialized for Urban application
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Viorela Ila
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "NonlinearFactor.h"
|
||||||
|
#include "GaussianFactor.h"
|
||||||
|
#include "Testable.h"
|
||||||
|
#include "Point2.h"
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
class UrbanConfig;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Non-linear factor for a constraint derived from a 2D measurement,
|
||||||
|
*/
|
||||||
|
class UrbanMeasurement : public NonlinearFactor<UrbanConfig>, Testable<UrbanMeasurement>
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
|
||||||
|
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:
|
||||||
|
|
||||||
|
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 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;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* predict the measurement (is that update ??)
|
||||||
|
*/
|
||||||
|
Vector predict(const UrbanConfig&) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* calculate the error of the factor
|
||||||
|
*/
|
||||||
|
Vector error_vector(const UrbanConfig&) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* linerarization
|
||||||
|
*/
|
||||||
|
|
||||||
|
GaussianFactor::shared_ptr linearize(const UrbanConfig&) const {
|
||||||
|
//TODO implement linearize
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
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_);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,99 @@
|
||||||
|
#! /bin/sh
|
||||||
|
|
||||||
|
# testUrbanGraph - temporary wrapper script for .libs/testUrbanGraph
|
||||||
|
# Generated by ltmain.sh - GNU libtool 1.5.22 (1.1220.2.365 2005/12/18 22:14:06)
|
||||||
|
#
|
||||||
|
# The testUrbanGraph program cannot be directly executed until all the libtool
|
||||||
|
# libraries that it depends on are installed.
|
||||||
|
#
|
||||||
|
# This wrapper script should never be moved out of the build directory.
|
||||||
|
# If it is, it will not operate correctly.
|
||||||
|
|
||||||
|
# Sed substitution that helps us do robust quoting. It backslashifies
|
||||||
|
# metacharacters that are still active within double-quoted strings.
|
||||||
|
Xsed='/usr/bin/sed -e 1s/^X//'
|
||||||
|
sed_quote_subst='s/\([\\`\\"$\\\\]\)/\\\1/g'
|
||||||
|
|
||||||
|
# The HP-UX ksh and POSIX shell print the target directory to stdout
|
||||||
|
# if CDPATH is set.
|
||||||
|
(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
|
||||||
|
|
||||||
|
relink_command=""
|
||||||
|
|
||||||
|
# This environment variable determines our operation mode.
|
||||||
|
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
|
||||||
|
# install mode needs the following variable:
|
||||||
|
notinst_deplibs=' libgtsam.la'
|
||||||
|
else
|
||||||
|
# When we are sourced in execute mode, $file and $echo are already set.
|
||||||
|
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
|
||||||
|
echo="/bin/echo"
|
||||||
|
file="$0"
|
||||||
|
# Make sure echo works.
|
||||||
|
if test "X$1" = X--no-reexec; then
|
||||||
|
# Discard the --no-reexec flag, and continue.
|
||||||
|
shift
|
||||||
|
elif test "X`($echo '\t') 2>/dev/null`" = 'X\t'; then
|
||||||
|
# Yippee, $echo works!
|
||||||
|
:
|
||||||
|
else
|
||||||
|
# Restart under the correct shell, and then maybe $echo will work.
|
||||||
|
exec /bin/sh "$0" --no-reexec ${1+"$@"}
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
# Find the directory that this script lives in.
|
||||||
|
thisdir=`$echo "X$file" | $Xsed -e 's%/[^/]*$%%'`
|
||||||
|
test "x$thisdir" = "x$file" && thisdir=.
|
||||||
|
|
||||||
|
# Follow symbolic links until we get to the real thisdir.
|
||||||
|
file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'`
|
||||||
|
while test -n "$file"; do
|
||||||
|
destdir=`$echo "X$file" | $Xsed -e 's%/[^/]*$%%'`
|
||||||
|
|
||||||
|
# If there was a directory component, then change thisdir.
|
||||||
|
if test "x$destdir" != "x$file"; then
|
||||||
|
case "$destdir" in
|
||||||
|
[\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
|
||||||
|
*) thisdir="$thisdir/$destdir" ;;
|
||||||
|
esac
|
||||||
|
fi
|
||||||
|
|
||||||
|
file=`$echo "X$file" | $Xsed -e 's%^.*/%%'`
|
||||||
|
file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'`
|
||||||
|
done
|
||||||
|
|
||||||
|
# Try to get the absolute directory name.
|
||||||
|
absdir=`cd "$thisdir" && pwd`
|
||||||
|
test -n "$absdir" && thisdir="$absdir"
|
||||||
|
|
||||||
|
program='testUrbanGraph'
|
||||||
|
progdir="$thisdir/.libs"
|
||||||
|
|
||||||
|
|
||||||
|
if test -f "$progdir/$program"; then
|
||||||
|
# Add our own library path to DYLD_LIBRARY_PATH
|
||||||
|
DYLD_LIBRARY_PATH="/Users/vila/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
|
||||||
|
|
||||||
|
# Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH
|
||||||
|
# The second colon is a workaround for a bug in BeOS R4 sed
|
||||||
|
DYLD_LIBRARY_PATH=`$echo "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
|
||||||
|
|
||||||
|
export DYLD_LIBRARY_PATH
|
||||||
|
|
||||||
|
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
|
||||||
|
# Run the actual program with our arguments.
|
||||||
|
|
||||||
|
exec "$progdir/$program" ${1+"$@"}
|
||||||
|
|
||||||
|
$echo "$0: cannot exec $program ${1+"$@"}"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
else
|
||||||
|
# The program doesn't exist.
|
||||||
|
$echo "$0: error: \`$progdir/$program' does not exist" 1>&2
|
||||||
|
$echo "This script is just a wrapper for $program." 1>&2
|
||||||
|
/bin/echo "See the libtool documentation for more information." 1>&2
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
@ -0,0 +1,108 @@
|
||||||
|
/**
|
||||||
|
* @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 "Pose3.h"
|
||||||
|
#include "Point2.h"
|
||||||
|
#include "Ordering.h"
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
typedef NonlinearOptimizer<UrbanGraph,UrbanConfig> Optimizer;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
Point2 landmark1( 2, 5);
|
||||||
|
Point2 landmark2( 2, 10);
|
||||||
|
Point2 landmark3( -2, 5);
|
||||||
|
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*/
|
||||||
|
Pose3 robotPose1(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.*/
|
||||||
|
Pose3 robotPose2(Matrix_(3,3,
|
||||||
|
0., 1., 0.,
|
||||||
|
1., 0., 0.,
|
||||||
|
0., 0.,-1.
|
||||||
|
),
|
||||||
|
Point3(0,1,0));
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
/* 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( 5, 2, sigma, 1, 1); // z11
|
||||||
|
g.addMeasurement(10, 2, sigma, 1, 2); // z12
|
||||||
|
g.addMeasurement( 5, -2, sigma, 1, 3); // z13
|
||||||
|
g.addMeasurement(10, -2, sigma, 1, 4); // z14
|
||||||
|
g.addOdometry(1, 0, sigmadx,sigmayaw, 1); // 1m forward, 0 yaw
|
||||||
|
g.addMeasurement( 4, 2, sigma, 2, 1); // z21
|
||||||
|
g.addMeasurement( 9, 2, sigma, 2, 2); // z22
|
||||||
|
g.addMeasurement( 4, -2, sigma, 2, 3); // z23
|
||||||
|
g.addMeasurement( 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, robotPose1);
|
||||||
|
initialEstimate->addRobotPose(2, robotPose2);
|
||||||
|
initialEstimate->addLandmark(1, landmark1);
|
||||||
|
initialEstimate->addLandmark(2, landmark2);
|
||||||
|
initialEstimate->addLandmark(3, landmark3);
|
||||||
|
initialEstimate->addLandmark(4, landmark4);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
//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();
|
||||||
|
// DOUBLES_EQUAL(0, optimizer.error(), 1e-9);
|
||||||
|
|
||||||
|
// check if correct
|
||||||
|
// CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;}
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue