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 += NonlinearEquality.h
|
||||
headers += Pose2Config.h Pose2Factor.h
|
||||
headers += UrbanGraph.h
|
||||
sources += NonlinearFactor.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_LDADD = libgtsam.la
|
||||
testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp
|
||||
|
|
@ -148,6 +150,8 @@ testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
|
|||
testPose2Factor_LDADD = libgtsam.la
|
||||
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
|
||||
testPose2Graph_LDADD = libgtsam.la
|
||||
testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp
|
||||
testUrbanGraph_LDADD = libgtsam.la
|
||||
|
||||
# geometry
|
||||
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 H = eye(6,6);
|
||||
return H;
|
||||
Matrix H = eye(6,6);
|
||||
return H;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse() const
|
||||
{
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt,-(Rt*t_));
|
||||
}
|
||||
{
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt,-(Rt*t_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::transformPose_to(const Pose3& pose) const
|
||||
{
|
||||
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
|
||||
Point3 t = transform_to(pose, t_);
|
||||
|
||||
return Pose3(cRv, t);
|
||||
}
|
||||
{
|
||||
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
|
||||
Point3 t = transform_to(pose, 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
|
||||
|
|
|
|||
20
cpp/Pose3.h
20
cpp/Pose3.h
|
|
@ -77,6 +77,19 @@ namespace gtsam {
|
|||
/** inverse transformation */
|
||||
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 */
|
||||
inline Pose3 operator*(const Pose3& B) const {
|
||||
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_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 */
|
||||
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