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

Fix issues to compile with class version of Point2.

release/4.3a0
Frank Dellaert 2016-06-06 22:26:44 -07:00
parent a97502f5a1
commit 3fda4dd33a
6 changed files with 46 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/geometry/CalibratedCamera.cpp
View File

@ -24,7 +24,7 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) {
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
@ -34,7 +34,7 @@ Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) {
}
/* ************************************************************************* */
Matrix23 PinholeBase::Dpoint(const Vector2& pn, double d, const Matrix3& Rt) {
Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
@ -85,20 +85,20 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
}
/* ************************************************************************* */
Vector2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) {
Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) {
double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Vector2(u, v);
return Point2(u, v);
}
/* ************************************************************************* */
Vector2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
if (Dpoint) {
Matrix32 Dpoint3_pc;
Matrix23 Duv_point3;
Vector2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
*Dpoint = Duv_point3 * Dpoint3_pc;
return uv;
} else
@ -106,14 +106,14 @@ Vector2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
}
/* ************************************************************************* */
pair<Vector2, bool> PinholeBase::projectSafe(const Point3& pw) const {
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Vector2 pn = Project(pc);
const Point2 pn = Project(pc);
return make_pair(pn, pc.z() > 0);
}
/* ************************************************************************* */
Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
@ -122,7 +122,7 @@ Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
if (q.z() <= 0)
throw CheiralityException();
#endif
const Vector2 pn = Project(q);
const Point2 pn = Project(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
@ -135,7 +135,7 @@ Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
}
/* ************************************************************************* */
Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const {
// world to camera coordinate
@ -146,7 +146,7 @@ Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
// camera to normalized image coordinate
Matrix2 Dpn_pc;
const Vector2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
// chain the Jacobian matrices
if (Dpose) {
@ -161,7 +161,7 @@ Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Vector2& p,
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth);
}
@ -178,7 +178,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
}
/* ************************************************************************* */
Vector2 CalibratedCamera::project(const Point3& point,
Point2 CalibratedCamera::project(const Point3& point,
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
return project2(point, Dcamera, Dpoint);
}

22
gtsam/geometry/CalibratedCamera.h
View File

@ -54,7 +54,7 @@ public:
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Vector2 Measurement;
typedef Point2 Measurement;
private:
@ -70,7 +70,7 @@ protected:
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
*/
static Matrix26 Dpose(const Vector2& pn, double d);
static Matrix26 Dpose(const Point2& pn, double d);
/**
* Calculate Jacobian with respect to point
@ -78,7 +78,7 @@ protected:
* @param d disparity (inverse depth)
* @param Rt transposed rotation matrix
*/
static Matrix23 Dpoint(const Vector2& pn, double d, const Matrix3& Rt);
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
/// @}
@ -169,7 +169,7 @@ public:
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Vector2 Project(const Point3& pc, //
static Point2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
/**
@ -177,18 +177,18 @@ public:
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Vector2 Project(const Unit3& pc, //
static Point2 Project(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none);
/// Project a point into the image and check depth
std::pair<Vector2, bool> projectSafe(const Point3& pw) const;
std::pair<Point2, bool> projectSafe(const Point3& pw) const;
/** Project point into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Vector2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Project point at infinity into the image
@ -196,12 +196,12 @@ public:
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Vector2 project2(const Unit3& point,
Point2 project2(const Unit3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 backproject_from_camera(const Vector2& p, const double depth);
static Point3 backproject_from_camera(const Point2& p, const double depth);
/// @}
/// @name Advanced interface
@ -325,11 +325,11 @@ public:
* @deprecated
* Use project2, which is more consistently named across Pinhole cameras
*/
Vector2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Vector2& pn, double depth) const {
Point3 backproject(const Point2& pn, double depth) const {
return pose().transform_from(backproject_from_camera(pn, depth));
}

14
gtsam/geometry/Point2.cpp
View File

@ -63,7 +63,7 @@ bool Point2::equals(const Point2& q, double tol) const {
/* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const {
return norm(*this, H);
return gtsam::norm2(*this, H);
}
/* ************************************************************************* */
@ -78,6 +78,18 @@ ostream &operator<<(ostream &os, const Point2& p) {
return os;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol) {
return circleCircleIntersection(R_d, r_d, tol);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh) {
return circleCircleIntersection(c1, c2, fh);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) {
return circleCircleIntersection(c1, r1, c2, r2, tol);
}
#endif
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */

20
gtsam/geometry/Point2.h
View File

@ -61,14 +61,6 @@ public:
/// construct from 2D vector
explicit Point2(const Vector2& v):Vector2(v) {}
/// @}
/// @name Declare circle intersection functionality
/// @{
friend boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol);
friend std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
friend std::list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol);
/// @}
/// @name Testable
/// @{
@ -132,15 +124,9 @@ public:
static Vector2 Logmap(const Point2& p) { return p;}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
inline double dist(const Point2& p2) const {return distance(p2);}
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9) {
return circleCircleIntersection( R_d, r_d, tol);
}
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>) {
return circleCircleIntersection( c1, c2, boost::optional<Point2>);
}
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9) {
return CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
}
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
/// @}
#endif

4
gtsam/geometry/Point3.cpp
View File

@ -34,11 +34,11 @@ void Point3::print(const string& s) const {
/* ************************************************************************* */
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::distance(*this,q,H1,H2);
return gtsam::distance3(*this,q,H1,H2);
}
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm(*this, H);
return gtsam::norm3(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {

6
gtsam/geometry/tests/testPoint2.cpp
View File

@ -111,9 +111,9 @@ TEST( Point2, arithmetic) {
/* ************************************************************************* */
TEST( Point2, unit) {
Point2 p0(10, 0), p1(0, -10), p2(10, 10);
EXPECT(assert_equal(Point2(1, 0), p0.normalized(), 1e-6));
EXPECT(assert_equal(Point2(0,-1), p1.normalized(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.normalized(), 1e-6));
EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6));
EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6));
}
namespace {