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

some changes after review 1

release/4.3a0
akrishnan86 2020-05-05 09:27:27 -04:00
parent 4356c1953d
commit 1c85b99b30
4 changed files with 167 additions and 248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

191
gtsam/geometry/Line3.cpp
View File

@ -1,136 +1,97 @@
#include <gtsam/geometry/Line3.h> #include <gtsam/geometry/Line3.h>
#include <cstdlib>
namespace gtsam { namespace gtsam {
Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const {
Vector6 Line3::points() Vector3 w;
{ w << v[0], v[1], 0;
Vector3 mid; Rot3 eps;
mid << a_, b_, 0; if (H) {
Vector3 mid_rot = R_*mid; OptionalJacobian<3, 3> Dw;
Vector3 start = mid_rot + R_.r3(); eps = Rot3::Expmap(w, Dw);
Vector3 end = mid_rot - R_.r3(); H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0);
Vector6 start_end; (*H)(2, 2) = 1;
start_end << start(0), start(1), start(2), (*H)(3, 3) = 1;
end(0), end(1), end(2); } else {
return start_end; eps = Rot3::Expmap(w);
}
Rot3 Rt = R_ * eps;
return Line3(Rt, a_ + v[2], b_ + v[3]);
} }
Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H) const {
Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4,4> H) const { Vector3 local_rot;
Vector3 w; Vector4 local;
w << v[0], v[1], 0; Vector2 ab_q(q.V());
Rot3 eps; if (H) {
if(H) OptionalJacobian<3, 3> Dw;
{ local_rot = Rot3::Logmap(R_.inverse() * q.R(), Dw);
OptionalJacobian<3,3> Dw; H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0);
eps = Rot3::Expmap(w, Dw); (*H)(2, 2) = 1;
H->block<2,2>(0,0) = Dw->block<2,2>(0,0); (*H)(3, 3) = 1;
(*H)(2,2) = 1; } else {
(*H)(3,3) = 1; local_rot = Rot3::Logmap(R_.inverse() * q.R());
} }
else local << local_rot[0], local_rot[1], ab_q[0] - a_, ab_q[1] - b_;
{ return local;
eps = Rot3::Expmap(w);
}
Rot3 Rt = R_*eps;
return Line3(Rt, a_+v[2], b_+v[3]);
} }
void Line3::print(const std::string &s) const {
Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4,4> H) const { std::cout << s << std::endl;
Vector3 local_rot; R_.print("R:\n");
Vector4 local; std::cout << "a: " << a_ << ", b: " << b_ << std::endl;
Vector2 ab_q(q.V());
if(H)
{
OptionalJacobian<3,3> Dw;
local_rot = Rot3::Logmap(R_.inverse()*q.R(), Dw);
H->block<2,2>(0, 0) = Dw->block<2,2>(0,0);
(*H)(2,2) = 1;
(*H)(3,3) = 1;
}
else
{
local_rot = Rot3::Logmap(R_.inverse()*q.R());
}
local << local_rot[0], local_rot[1], ab_q[0] - a_, ab_q[1] - b_;
return local;
} }
bool Line3::equals(const Line3 &l2, double tol) const {
void Line3::print(const std::string &s) const Vector4 diff = localCoordinates(l2);
{ return fabs(diff[0]) < tol && fabs(diff[1]) < tol
std::cout << s << std::endl; && fabs(diff[2]) < tol && fabs(diff[3]) < tol;
R_.print("R:\n");
std::cout << "a: " << a_ << ", b: " << b_ << std::endl;
} }
Point3 Line3::project(OptionalJacobian<3, 4> Dline) const {
Vector3 V_0;
V_0 << -b_, a_, 0;
bool Line3::equals(const Line3 &l2, double tol) const Unit3 l = R_ * V_0;
{ if (Dline) {
Vector4 diff = localCoordinates(l2); Dline->setZero();
return fabs(diff[0]) < tol && fabs(diff[1]) < tol Dline->col(0) = a_ * R_.r3();
&& fabs(diff[2]) < tol && fabs(diff[3]) < tol; Dline->col(1) = b_ * R_.r3();
Dline->col(2) = R_.r2();
Dline->col(3) = -R_.r1();
}
return l;
} }
Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) {
Rot3 wRl = wL.R();
Rot3 wRc = wTc.rotation();
Rot3 cRw = wRc.inverse();
Rot3 cRl = cRw * wRl;
Point3 Line3::project(OptionalJacobian<3, 4> Dline) const Vector2 w_ab = wL.V();
{ Vector3 t = (wRl.transpose() * wTc.translation());
Vector3 V_0; Vector2 c_ab(w_ab[0] - t[0], w_ab[1] - t[1]);
V_0 << -b_, a_, 0;
Point3 l = R_*V_0; if (Dpose) {
if(Dline) // translation due to translation
{ Matrix3 cRl_mat = cRl.matrix();
Dline->setZero(); Matrix3 lRc = cRl_mat.transpose();
Dline->col(0) = a_*R_.r3(); Dpose->block<1, 3>(2, 3) = -lRc.row(0);
Dline->col(1) = b_*R_.r3(); Dpose->block<1, 3>(3, 3) = -lRc.row(1);
Dline->col(2) = R_.r2();
Dline->col(3) = -R_.r1();
}
return l;
}
Dpose->block<1, 3>(0, 0) = -lRc.row(0);
Point3 projectLine(const Line3 &L, Dpose->block<1, 3>(1, 0) = -lRc.row(1);
OptionalJacobian<3, 4> Dline) }
{ if (Dline) {
return L.project(Dline); Dline->col(0) << 1.0, 0.0, 0.0, -t[2];
} Dline->col(1) << 0.0, 1.0, t[2], 0.0;
Dline->col(2) << 0.0, 0.0, 1.0, 0.0;
Dline->col(3) << 0.0, 0.0, 0.0, 1.0;
Line3 transformTo(const Pose3 &p, const Line3 &l, }
OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) return Line3(cRl, c_ab[0], c_ab[1]);
{
Rot3 w_R_l = l.R();
Rot3 w_R_c = p.rotation();
Rot3 c_R_w = w_R_c.inverse();
Rot3 c_R_l = c_R_w*w_R_l;
Vector2 ab_w = l.V();
Vector3 t = (w_R_l.transpose()*p.translation());
Vector2 ab_c(ab_w[0] - t[0], ab_w[1] - t[1]);
if(Dpose)
{
// translation due to translation
Matrix3 c_R_l_mat = c_R_l.matrix();
Matrix3 l_R_c = c_R_l_mat.transpose();
Dpose->block<1,3>(2, 3) = -l_R_c.row(0);
Dpose->block<1,3>(3, 3) = -l_R_c.row(1);
Dpose->block<1,3>(0, 0) = -l_R_c.row(0);
Dpose->block<1,3>(1, 0) = -l_R_c.row(1);
}
if(Dline)
{
Dline->col(0) << 1.0, 0.0, 0.0, -t[2];
Dline->col(1) << 0.0, 1.0, t[2], 0.0;
Dline->col(2) << 0.0, 0.0, 1.0, 0.0;
Dline->col(3) << 0.0, 0.0, 0.0, 1.0;
}
return Line3(c_R_l, ab_c[0], ab_c[1]);
} }
} }

202
gtsam/geometry/Line3.h
View File

@ -18,15 +18,9 @@
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/expressions.h>
#include <boost/random.hpp> namespace gtsam {
#include <cstdlib>
#include <ctime>
namespace gtsam{
/** /**
* A 3D line (R,a,b) : (Rot3,Scalar,Scalar) * A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
@ -34,145 +28,99 @@ namespace gtsam{
* \nosubgrouping * \nosubgrouping
*/ */
class Line3 { class Line3 {
private: private:
Rot3 R_; // Rotation of line about x and y in world frame Rot3 R_; // Rotation of line about x and y in world frame
double a_, b_; // Intersection of line with x-y world plane double a_, b_; // Intersection of line with the world x-y plane rotated by R_
// Also the closest point on line to origin
public:
enum { dimension = 4 };
public: /** Default constructor is the Z axis **/
enum {dimension = 4}; Line3() :
a_(0), b_(0) {}
/** Default constructor is the Z axis **/ /** Constructor:
Line3() : * Parallel to z axis, intersecting x-y plane at (a,b) **/
a_(0), b_(0) Line3(const double a, const double b) :
{} a_(a), b_(b) {}
/** Constructor: /** Constructor for general line from (R, a, b) **/
* Parallel to z axis, intersecting x-y plane at (a,b) **/ Line3(const Rot3 &R, const double a, const double b) :
Line3(const double a, const double b): R_(R), a_(a), b_(b) {}
a_(a), b_(b)
{}
/** Constructor for general line from (R, a, b) **/ /**
Line3(const Rot3 &R, const double a, const double b) : * The retract method maps from the tangent space back to the manifold.
R_(R), a_(a), b_(b) * The tangent space for the rotation of a line is only two dimensional -
{} * rotation about x and y
* @param v: increment in tangent space
* @param H: Jacobian of retraction with respect to the increment
* @return: resulting line after adding the increment and mapping to the manifold
*/
Line3 retract(const Vector4 &v, OptionalJacobian<4, 4> H = boost::none) const;
/** Sample two points lying on the line /**
* @return Vector6 where first 3 elements are one point, and * The localCoordinates method is the inverse of retract and finds the difference
* next 3 elements are another point **/ * between two lines in the tangent space.
Vector6 points(); * @param q Line3 on manifold
* @param H OptionalJacobian of localCoordinates with respect to line
* @return difference in the tangent space
*/
Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H = boost::none) const;
/** /**
* The retract method maps from the tangent space back to the manifold. * Rotation of line accessor
* The tangent space for the rotation of a line is only two dimensional - * @return Rot3
* rotation about x and y */
* @param v: increment in tangent space Rot3 R() const {
* @param H: Jacobian of retraction with respect to the increment return R_;
* @return: resulting line after adding the increment and mapping to the manifold }
*/
Line3 retract(const Vector4 &v, OptionalJacobian<4,4> H = boost::none) const;
/** /**
* The localCoordinates method is the inverse of retract and finds the difference * Accessor for a, b
* between two lines in the tangent space. * @return Vector2(a, b)
* @param q Line3 on manifold */
* @param H OptionalJacobian of localCoordinates with respect to line Vector2 V() const {
* @return Vector4 difference in the tangent space return Vector2(a_, b_);
*/ }
Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4,4> H = boost::none) const;
/** /**
* Rotation of line accessor * Print R, a, b
* @return Rot3 * @param s: optional starting string
*/ */
Rot3 R() const void print(const std::string &s = "") const;
{
return R_;
}
/** /**
* Accessor for a, b * Check if two lines are equal
* @return Vector2(a, b) * @param l2 - line to be compared
*/ * @param tol : optional tolerance
Vector2 V() const * @return boolean - true if lines are equal
{ */
return Vector2(a_, b_); bool equals(const Line3 &l2, double tol = 10e-9) const;
}
/** /**
* Print R, a, b * Projecting a line to the image plane. Assumes this line is in camera frame.
* @param s: optional starting string * @param Dline: OptionalJacobian of projected line with respect to this line
*/ * @return Unit3 - projected line in image plane, in homogenous coordinates.
void print(const std::string &s = "") const; * We use Unit3 since it is a manifold with the right dimension.
*/
/** Point3 project(OptionalJacobian<3, 4> Dline = boost::none) const;
* Check if two lines are equal
* @param l2 - line to be compared
* @param tol : optional tolerance
* @return boolean - true if lines are equal
*/
bool equals(const Line3 &l2, double tol = 10e-9) const;
/**
* Projecting a line to an image plane
* @param Dline: OptionalJacobian of projected line with respect to this line
* @return Point3 - projected line in image plane
*/
Point3 project(OptionalJacobian<3, 4> Dline = boost::none) const;
}; };
template <> template<>
struct traits<Line3> : public internal::Manifold<Line3> {}; struct traits<Line3> : public internal::Manifold<Line3> {};
template <> template<>
struct traits<const Line3> : public internal::Manifold<Line3> {}; struct traits<const Line3> : public internal::Manifold<Line3> {};
// For expression factor graphs
typedef Expression<Line3> Line3_;
/**
* Function projects input line to image plane
* @param L input line
* @param Dline OptionalJacbian of projected line with respect to input line
* @return Point3 - projected line
*/
Point3 projectLine(const Line3 &L,
OptionalJacobian<3, 4> Dline = boost::none);
/** /**
* Transform a line from world to camera frame * Transform a line from world to camera frame
* @param p - Pose3 of camera in world frame * @param wTc - Pose3 of camera in world frame
* @param l - Line3 in world frame * @param wL - Line3 in world frame
* @param Dpose - OptionalJacobian of transformed line with respect to p * @param Dpose - OptionalJacobian of transformed line with respect to p
* @param Dline - OptionalJacobian of transformed line with respect to l * @param Dline - OptionalJacobian of transformed line with respect to l
* @return Transformed line in camera frame * @return Transformed line in camera frame
*/ */
Line3 transformTo(const Pose3 &p, const Line3 &l, Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
OptionalJacobian<4, 6> Dpose = boost::none, OptionalJacobian<4, 6> Dpose = boost::none,
OptionalJacobian<4, 4> Dline = boost::none); OptionalJacobian<4, 4> Dline = boost::none);
/**
* Expression method for projection
* @param line Line3_ expression
* @return Point3_ projected line
*/
inline Point3_ projectLineExpression(const Line3_& line)
{
Point3 (*f)(const Line3&, OptionalJacobian<3, 4>) = &projectLine;
return Point3_(f, line);
}
/**
* Expression method for transforming line
* @param p - Pose3_ expression
* @param line - Line3 expression
* @return transformed Line3_ expression
*/
inline Line3_ transformToExpression(const Pose3_& p, const Line3_& line)
{
Line3 (*f)(const Pose3&, const Line3&,
OptionalJacobian<4,6>, OptionalJacobian<4,4>) = &transformTo;
return Line3_(f, p, line);
}
} }

14
gtsam/geometry/tests/testLine3.cpp
View File

@ -2,6 +2,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Line3.h> #include <gtsam/geometry/Line3.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ExpressionFactor.h> #include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressionTesting.h> #include <gtsam/nonlinear/expressionTesting.h>
@ -117,7 +118,7 @@ TEST(Line3, transformToExpressionJacobians)
val.insert(2, p); val.insert(2, p);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1);
ExpressionFactor<Line3> f(model, transformTo(p, l_w), transformToExpression(p_, l_)); ExpressionFactor<Line3> f(model, transformTo(p, l_w), transformTo(p_, l_));
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
} }
@ -127,17 +128,18 @@ TEST(Line3, projection)
{ {
Rot3 r; Rot3 r;
r = r.Expmap(Vector3(0, 0, 0)); r = r.Expmap(Vector3(0, 0, 0));
Line3 l_w(r, 1, 1); Line3 wL(r, 1, 1);
Point3 expected(-1, 1, 0); Point3 expected(-1, 1, 0);
EXPECT(expected.equals(projectLine(l_w))); EXPECT(expected.equals(wL.project()));
Values val; Values val;
val.insert(1, l_w); val.insert(1, wL);
Line3_ l_w_(1); Line3_ wL_(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
ExpressionFactor<Point3> f(model, expected, projectLineExpression(l_w_)); Point3_ projected_(wL_, &Line3::project);
ExpressionFactor<Point3> f(model, expected, projected_);
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
} }

8
gtsam/slam/expressions.h
View File

@ -11,6 +11,7 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Line3.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
namespace gtsam { namespace gtsam {
@ -31,6 +32,7 @@ typedef Expression<Point3> Point3_;
typedef Expression<Unit3> Unit3_; typedef Expression<Unit3> Unit3_;
typedef Expression<Rot3> Rot3_; typedef Expression<Rot3> Rot3_;
typedef Expression<Pose3> Pose3_; typedef Expression<Pose3> Pose3_;
typedef Expression<Line3> Line3_;
inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transformTo, p); return Point3_(x, &Pose3::transformTo, p);
@ -40,6 +42,12 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transformFrom, p); return Point3_(x, &Pose3::transformFrom, p);
} }
inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
Line3 (*f)(const Pose3 &, const Line3 &,
OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo;
return Line3_(f, wTc, wL);
}
namespace internal { namespace internal {
// define getter that returns value rather than reference // define getter that returns value rather than reference
inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {