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

No more LieVector/LieScalar

release/4.3a0
dellaert 2014-11-03 13:32:58 +01:00
parent 39ce31d0cc
commit 2a745b6c26
9 changed files with 54 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -69,10 +69,9 @@ public:
* @param inv inverse depth
* @return Point3
*/
static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) {
gtsam::Point3 ray_base(pw.vector().segment(0,3));
static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
gtsam::Point3 ray_base(pw.segment(0,3));
double theta = pw(3), phi = pw(4);
double rho = inv.value(); // inverse depth
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
return ray_base + m/rho;
}
@ -82,15 +81,14 @@ public:
* @param H1 is the jacobian w.r.t. [pose3 calibration]
* @param H2 is the jacobian w.r.t. inv_depth_landmark
*/
inline gtsam::Point2 project(const gtsam::LieVector& pw,
const gtsam::LieScalar& inv_depth,
inline gtsam::Point2 project(const Vector5& pw,
double rho,
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const {
gtsam::Point3 ray_base(pw.vector().segment(0,3));
gtsam::Point3 ray_base(pw.segment(0,3));
double theta = pw(3), phi = pw(4);
double rho = inv_depth.value(); // inverse depth
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
const gtsam::Point3 landmark = ray_base + m/rho;
@ -155,7 +153,7 @@ public:
* useful for point initialization
*/
inline std::pair<gtsam::LieVector, gtsam::LieScalar> backproject(const gtsam::Point2& pi, const double depth) const {
inline std::pair<Vector5, double> backproject(const gtsam::Point2& pi, const double depth) const {
const gtsam::Point2 pn = k_->calibrate(pi);
gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
pc = pc/pc.norm();
@ -164,8 +162,8 @@ public:
gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
gtsam::LieScalar(1./depth));
return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
double(1./depth));
}
private:

2
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -80,7 +80,7 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth,
gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {

8
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
/**
* Binary factor representing a visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, LieVector> {
class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -32,7 +32,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor2<Pose3, LieVector> Base;
typedef NoiseModelFactor2<Pose3, Vector6> Base;
/// shorthand for this class
typedef InvDepthFactorVariant1 This;
@ -78,7 +78,7 @@ public:
&& this->K_->equals(*e->K_, tol);
}
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
Vector inverseDepthError(const Pose3& pose, const Vector6& landmark) const {
try {
// Calculate the 3D coordinates of the landmark in the world frame
double x = landmark(0), y = landmark(1), z = landmark(2);
@ -99,7 +99,7 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const LieVector& landmark,
Vector evaluateError(const Pose3& pose, const Vector6& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {

8
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
/**
* Binary factor representing a visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, LieVector> {
class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor2<Pose3, LieVector> Base;
typedef NoiseModelFactor2<Pose3, Vector3> Base;
/// shorthand for this class
typedef InvDepthFactorVariant2 This;
@ -82,7 +82,7 @@ public:
&& this->referencePoint_.equals(e->referencePoint_, tol);
}
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
try {
// Calculate the 3D coordinates of the landmark in the world frame
double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@ -102,7 +102,7 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const LieVector& landmark,
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {

27
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
/**
* Binary factor representing the first visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, LieVector> {
class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -32,7 +32,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor2<Pose3, LieVector> Base;
typedef NoiseModelFactor2<Pose3, Vector3> Base;
/// shorthand for this class
typedef InvDepthFactorVariant3a This;
@ -80,7 +80,7 @@ public:
&& this->K_->equals(*e->K_, tol);
}
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
try {
// Calculate the 3D coordinates of the landmark in the Pose frame
double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@ -102,7 +102,7 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const LieVector& landmark,
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
@ -141,7 +141,7 @@ private:
/**
* Ternary factor representing a visual measurement using an inverse-depth parameterization
*/
class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, LieVector> {
class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -151,7 +151,7 @@ protected:
public:
/// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
typedef NoiseModelFactor3<Pose3, Pose3, Vector3> Base;
/// shorthand for this class
typedef InvDepthFactorVariant3b This;
@ -199,7 +199,7 @@ public:
&& this->K_->equals(*e->K_, tol);
}
Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark) const {
Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
try {
// Calculate the 3D coordinates of the landmark in the Pose1 frame
double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@ -221,20 +221,19 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark,
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
if(H1) {
if(H1)
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
}
if(H2) {
if(H2)
(*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
}
if(H3) {
if(H3)
(*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
}
return inverseDepthError(pose1, pose2, landmark);
}

10
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -25,7 +25,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
typedef InvDepthFactor3<Pose3, LieVector, LieScalar> InverseDepthFactor;
typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
typedef NonlinearEquality<Pose3> PoseConstraint;
/* ************************************************************************* */
@ -38,10 +38,10 @@ TEST( InvDepthFactor, optimize) {
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
// initialize inverse depth with "incorrect" depth of 1/4
// in reality this is 1/5, but initial depth is guessed
LieScalar inv_depth(1./4);
double inv_depth(1./4);
gtsam::NonlinearFactorGraph graph;
Values initial;
@ -82,8 +82,8 @@ TEST( InvDepthFactor, optimize) {
Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
Point3 result2_lmk = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
result2.at<LieVector>(Symbol('l',1)),
result2.at<LieScalar>(Symbol('d',1)));
result2.at<Vector5>(Symbol('l',1)),
result2.at<double>(Symbol('d',1)));
EXPECT(assert_equal(landmark, result2_lmk, 1e-9));
// TODO: need to add priors to make this work with

14
gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
View File

@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) {
double theta = atan2(ray.y(), ray.x());
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
double rho = 1./ray.norm();
LieVector expected((Vector(6) << x, y, z, theta, phi, rho));
Vector6 expected((Vector(6) << x, y, z, theta, phi, rho));
@ -68,12 +68,12 @@ TEST( InvDepthFactorVariant1, optimize) {
Values values;
values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
values.insert(landmarkKey, Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey);
Vector6 actual = result.at<Vector6>(landmarkKey);
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
@ -84,22 +84,22 @@ TEST( InvDepthFactorVariant1, optimize) {
// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
// pose2.print("Pose2 Truth:\n");
// cout << endl << endl;
// values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n");
// values.at<Vector6>(landmarkKey).print("Landmark Before:\n");
// result.at<Vector6>(landmarkKey).print("Landmark After:\n");
// expected.print("Landmark Truth:\n");
// cout << endl << endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
{
LieVector landmarkBefore = values.at<LieVector>(landmarkKey);
Vector6 landmarkBefore = values.at<Vector6>(landmarkKey);
double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
Point3 world_landmarkAfter;
{
LieVector landmarkAfter = result.at<LieVector>(landmarkKey);
Vector6 landmarkAfter = result.at<Vector6>(landmarkKey);
double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);
double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5);
world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);

16
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) {
double theta = atan2(ray.y(), ray.x());
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
double rho = 1./ray.norm();
LieVector expected((Vector(3) << theta, phi, rho));
Vector3 expected((Vector(3) << theta, phi, rho));
@ -66,12 +66,12 @@ TEST( InvDepthFactorVariant2, optimize) {
Values values;
values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05)));
values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey);
Vector3 actual = result.at<Vector3>(landmarkKey);
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
@ -81,21 +81,21 @@ TEST( InvDepthFactorVariant2, optimize) {
// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
// pose2.print("Pose2 Truth:\n");
// std::cout << std::endl << std::endl;
// values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n");
// values.at<Vector3>(landmarkKey).print("Landmark Before:\n");
// result.at<Vector3>(landmarkKey).print("Landmark After:\n");
// expected.print("Landmark Truth:\n");
// std::cout << std::endl << std::endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
{
LieVector landmarkBefore = values.at<LieVector>(landmarkKey);
Vector3 landmarkBefore = values.at<Vector3>(landmarkKey);
double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
Point3 world_landmarkAfter;
{
LieVector landmarkAfter = result.at<LieVector>(landmarkKey);
Vector3 landmarkAfter = result.at<Vector3>(landmarkKey);
double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);
world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
@ -106,7 +106,7 @@ TEST( InvDepthFactorVariant2, optimize) {
// std::cout << std::endl << std::endl;
// Test that the correct landmark parameters have been recovered
EXPECT(assert_equal(expected, actual, 1e-9));
EXPECT(assert_equal((Vector)expected, actual, 1e-9));
}

8
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) {
double theta = atan2(landmark_p1.x(), landmark_p1.z());
double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
double rho = 1./landmark_p1.norm();
LieVector expected((Vector(3) << theta, phi, rho));
Vector3 expected((Vector(3) << theta, phi, rho));
@ -66,17 +66,17 @@ TEST( InvDepthFactorVariant3, optimize) {
Values values;
values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05)));
values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey);
Vector3 actual = result.at<Vector3>(landmarkKey);
// Test that the correct landmark parameters have been recovered
EXPECT(assert_equal(expected, actual, 1e-9));
EXPECT(assert_equal((Vector)expected, actual, 1e-9));
}