better error message, formatting
Frank Dellaert
parent
0194926dc2
commit
edb72d305f
|
|
@ -86,19 +86,13 @@ bool UrbanConfig::equals(const UrbanConfig& c, double tol) const {
|
|||
/* ************************************************************************* */
|
||||
void UrbanConfig::addRobotPose(const int i, Pose3 cp)
|
||||
{
|
||||
pair<int, Pose3> robot;
|
||||
robot.first = i;
|
||||
robot.second = cp;
|
||||
robotPoses_.insert(robot);
|
||||
robotPoses_.insert(make_pair(i,cp));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void UrbanConfig::addLandmark(const int i, Point2 lp)
|
||||
void UrbanConfig::addLandmark(const int j, Point2 lp)
|
||||
{
|
||||
pair<int, Point2> landmark;
|
||||
landmark.first = i;
|
||||
landmark.second = lp;
|
||||
landmarkPoints_.insert(landmark);
|
||||
landmarkPoints_.insert(make_pair(j,lp));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
*/
|
||||
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include "Testable.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "Point2.h"
|
||||
|
|
@ -13,106 +14,120 @@
|
|||
|
||||
#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_){}
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Exponential map: takes 6D vectors in VectorConfig
|
||||
* and applies them to the poses in the UrbanConfig.
|
||||
* Needed for use in nonlinear optimization
|
||||
* Config that knows about points and poses
|
||||
*/
|
||||
UrbanConfig exmap(const VectorConfig & delta) const;
|
||||
class UrbanConfig: Testable<UrbanConfig> {
|
||||
|
||||
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();}
|
||||
private:
|
||||
typedef std::map<int, Pose3> PoseMap;
|
||||
typedef std::map<int, Point2> PointMap;
|
||||
PointMap landmarkPoints_;
|
||||
PoseMap robotPoses_;
|
||||
|
||||
/**
|
||||
* print
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
static std::string symbol(char c, int index) {
|
||||
std::stringstream ss;
|
||||
ss << c << index;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve robot pose
|
||||
*/
|
||||
bool robotPoseExists(int i) const
|
||||
{
|
||||
PoseMap::const_iterator it = robotPoses_.find(i);
|
||||
if (it==robotPoses_.end())
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
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() {
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
/*
|
||||
* copy constructor
|
||||
*/
|
||||
UrbanConfig(const UrbanConfig& original) :
|
||||
robotPoses_(original.robotPoses_), landmarkPoints_(
|
||||
original.landmarkPoints_) {
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
/**
|
||||
* 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;
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
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();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
/**
|
||||
* print
|
||||
*/
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
void removeRobotPose(const int i);
|
||||
void removeLandmark(const int i);
|
||||
/**
|
||||
* Retrieve robot pose
|
||||
*/
|
||||
bool robotPoseExists(int i) const {
|
||||
PoseMap::const_iterator it = robotPoses_.find(i);
|
||||
return (it != robotPoses_.end());
|
||||
}
|
||||
|
||||
void clear() {landmarkPoints_.clear(); robotPoses_.clear();}
|
||||
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;
|
||||
}
|
||||
|
||||
inline size_t size(){
|
||||
return landmarkPoints_.size() + robotPoses_.size();
|
||||
}
|
||||
};
|
||||
/**
|
||||
* 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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue