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

error_vector and error unit-tested

release/4.3a0
Frank Dellaert 2009-12-18 05:18:31 +00:00
parent edb72d305f
commit eaf27af92d
8 changed files with 223 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/UrbanFactor.cpp
View File

@ -11,7 +11,11 @@ namespace gtsam {
UrbanFactor::UrbanFactor() {
// TODO Auto-generated constructor stub
}
UrbanFactor::UrbanFactor(const Vector& z, const double sigma) :
NonlinearFactor<UrbanConfig> (z,sigma) {
// TODO
}
UrbanFactor::~UrbanFactor() {

17
cpp/UrbanFactor.h
View File

@ -13,20 +13,15 @@
namespace gtsam {
class UrbanFactor : public NonlinearFactor<UrbanConfig> {
/**
* Base class for UrbanMeasurement and UrbanOdometry
*/
class UrbanFactor: public NonlinearFactor<UrbanConfig> {
public:
UrbanFactor();
UrbanFactor(const Vector& z, const double sigma);
virtual ~UrbanFactor();
/** Vector of errors */
Vector error_vector(const UrbanConfig& c) const { return zero(0); }
/** linearize to a GaussianFactor */
boost::shared_ptr<GaussianFactor> linearize(const UrbanConfig& c) const {
boost::shared_ptr<GaussianFactor> factor(new GaussianFactor);
return factor;
}
};
}

7
cpp/UrbanGraph.cpp
View File

@ -33,9 +33,10 @@ namespace gtsam {
}
/* ************************************************************************* */
void UrbanGraph::addMeasurement(double x, double y, double sigma, int p1,
int p2) {
// TODO
void UrbanGraph::addMeasurement(double x, double y, double sigma, int i, int j) {
Point2 z(x,y);
sharedFactor factor(new UrbanMeasurement(z,sigma,i,j));
push_back(factor);
}
;

7
cpp/UrbanGraph.h
View File

@ -8,7 +8,7 @@
#pragma once
#include "NonlinearFactorGraph.h"
#include "UrbanFactor.h"
#include "UrbanMeasurement.h"
namespace gtsam {
@ -35,13 +35,12 @@ namespace gtsam {
/**
* Add a landmark constraint
*/
void addMeasurement(double x, double y, double sigma, int p1, int p2);
void addMeasurement(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 p);
void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int i);
/**
* Add an initial constraint on the first pose

122
cpp/UrbanMeasurement.cpp
View File

@ -5,71 +5,81 @@
* @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");
}
namespace gtsam {
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
Point2 transform_to(const Pose3& pose, const Point2& p) {
// create a 3D point at our height (Z is assumed up)
Point3 global3D(p.x(),p.y(),pose.translation().z());
// transform into local 3D point
Point3 local3D = transform_to(pose,global3D);
// take x and y as the local measurement
Point2 local2D(local3D.x(),local3D.y());
return local2D;
}
/* ************************************************************************* */
Matrix Dtransform_to1(const Pose3& pose, const Point2& p) {
return zeros(2, 6);
}
// 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;
}
/* ************************************************************************* */
Matrix Dtransform_to2(const Pose3& pose, const Point2& p) {
return zeros(2, 2);
}
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
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 i,
int j) :
UrbanFactor(z.vector(), sigma) {
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
Point2 hx = transform_to(pose,landmark);
return (z_ - hx.vector());
}
/* ************************************************************************* */

145
cpp/UrbanMeasurement.h
View File

@ -7,94 +7,93 @@
#pragma once
#include "NonlinearFactor.h"
#include "GaussianFactor.h"
#include "Testable.h"
#include "Point2.h"
#include "UrbanFactor.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;
class UrbanConfig;
/**
* Default constructor
* Non-linear factor for a constraint derived from a 2D measurement,
*/
UrbanMeasurement();
class UrbanMeasurement: public UrbanFactor {
private:
/**
* 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);
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:
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s="UrbanMeasurement") const;
typedef boost::shared_ptr<UrbanMeasurement> shared_ptr; // shorthand for a smart pointer to a factor
//typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
/**
* equals
*/
bool equals(const UrbanMeasurement&, double tol=1e-9) const;
/**
* Default constructor
*/
UrbanMeasurement();
/**
* predict the measurement (is that update ??)
*/
Vector predict(const UrbanConfig&) const;
/**
* 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);
/**
* calculate the error of the factor
*/
Vector error_vector(const UrbanConfig&) const;
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "UrbanMeasurement") const;
/**
* linerarization
*/
/**
* equals
*/
bool equals(const UrbanMeasurement&, double tol = 1e-9) const;
GaussianFactor::shared_ptr linearize(const UrbanConfig&) const {
//TODO implement linearize
/**
* 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_);
}
};
int getrobotPoseNumber() const { return robotPoseNumber_; }
int getLandmarkNumber() const { return landmarkNumber_; }
/**
* Transform 2D landmark into 6D pose, and its derivatives
*/
Point2 transform_to(const Pose3& pose, const Point2& p);
Matrix Dtransform_to1(const Pose3& pose, const Point2& p);
Matrix Dtransform_to2(const Pose3& pose);
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_);
}
};
}
} // namespace gtsam

34
cpp/testUrbanGraph.cpp
View File

@ -21,7 +21,7 @@ typedef NonlinearOptimizer<UrbanGraph,UrbanConfig> Optimizer;
/* ************************************************************************* */
Point2 landmark1( 2, 5);
Point2 landmark( 2, 5);
Point2 landmark2( 2, 10);
Point2 landmark3( -2, 5);
Point2 landmark4( -2,-10);
@ -29,7 +29,7 @@ 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,
Pose3 robotPose(Matrix_(3,3,
0., 1., 0.,
1., 0., 0.,
0., 0.,-1.
@ -45,8 +45,28 @@ Pose3 robotPose2(Matrix_(3,3,
Point3(0,1,0));
/* ************************************************************************* */
/* the measurements are relative to the robot pose so
* they are in robot coordinate frame different from the global (above)*/
TEST( UrbanGraph, addMeasurement)
{
// Check adding a measurement
UrbanGraph g;
double sigma = 0.2;// 20 cm
g.addMeasurement(4, 2, sigma, 1, 1);
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);
}
/* ************************************************************************* *
* 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
@ -68,7 +88,7 @@ UrbanGraph testGraph() {
return g;
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( UrbanGraph, optimizeLM)
{
// build a graph
@ -76,9 +96,9 @@ TEST( UrbanGraph, optimizeLM)
// Create an initial configuration corresponding to the ground truth
boost::shared_ptr<UrbanConfig> initialEstimate(new UrbanConfig);
initialEstimate->addRobotPose(1, robotPose1);
initialEstimate->addRobotPose(1, robotPose);
initialEstimate->addRobotPose(2, robotPose2);
initialEstimate->addLandmark(1, landmark1);
initialEstimate->addLandmark(1, landmark);
initialEstimate->addLandmark(2, landmark2);
initialEstimate->addLandmark(3, landmark3);
initialEstimate->addLandmark(4, landmark4);

42
cpp/testUrbanMeasurement.cpp
View File

@ -6,13 +6,53 @@
#include "UrbanConfig.h"
#include "UrbanMeasurement.h"
#include "Vector.h"
using namespace std;
using namespace gtsam;
Point2 landmark(2,5);
/* 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 robotPose(Matrix_(3,3,
0., 1., 0.,
1., 0., 0.,
0., 0.,-1.
),
Point3(0,0,0));
/* ************************************************************************* */
TEST( UrbanMeasurement, linearize )
TEST( UrbanMeasurement, transform_to )
{
Point2 expected(5,2);
Point2 actual = transform_to(robotPose, landmark);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( UrbanMeasurement, error_vector )
{
// Create a measurement, no-noise measurement would yield 5,2
Point2 z(4,2);
double sigma = 0.2;
UrbanMeasurement factor(z,sigma,44,66);
// Create a config
UrbanConfig config;
config.addRobotPose(44,robotPose);
config.addLandmark(66,landmark);
// test error_vector
Vector expected = Vector_(2, -1.0, 0.0);
Vector actual = factor.error_vector(config);
CHECK(assert_equal(expected,actual));
// test error
double expected2 = 12.5; // 0.5 * 5^2
double actual2 = factor.error(config);
DOUBLES_EQUAL(expected2,actual2,1e-9);
}
/* ************************************************************************* */