12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

better error message, formatting

release/4.3a0
Frank Dellaert 2009-12-18 05:16:09 +00:00
parent 0194926dc2
commit edb72d305f
2 changed files with 105 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/UrbanConfig.cpp
View File

@ -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));
}
/* ************************************************************************* */

189
cpp/UrbanConfig.h
View File

@ -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