Skip to content

Commit 19ecf50

Browse files
xingyc15xingyc15
committed
Run Clang-Format with Google style
1 parent c8431b3 commit 19ecf50

19 files changed

+2661
-2606
lines changed

‎include/omnimapper/BoundedPlane3.h

Lines changed: 118 additions & 129 deletions
Original file line numberDiff line numberDiff line change
@@ -1,138 +1,127 @@
11
#pragma once
22

3-
#include <gtsam/geometry/Rot3.h>
43
#include <gtsam/geometry/OrientedPlane3.h>
4+
#include <gtsam/geometry/Rot3.h>
5+
#include <gtsam/geometry/Unit3.h>
56
#include <omnimapper/DerivedValue.h>
67
#include <pcl/point_cloud.h>
78
#include <pcl/point_types.h>
8-
#include <gtsam/geometry/Unit3.h>
99

1010
#include <boost/thread/mutex.hpp>
1111

12-
namespace omnimapper
13-
{
14-
/**
15-
* A planar landmark bounded by a polygonal point cloud.
16-
*/
17-
template <typename PointT>
18-
class BoundedPlane3 : public gtsam::DerivedValue<BoundedPlane3<PointT> >
19-
{
20-
typedef typename pcl::PointCloud<PointT> Cloud;
21-
typedef typename Cloud::Ptr CloudPtr;
22-
typedef typename Cloud::ConstPtr CloudConstPtr;
23-
typedef typename boost::shared_ptr<boost::mutex> MutexPtr;
24-
25-
protected:
26-
gtsam::Unit3 n_;
27-
double d_;
28-
29-
CloudPtr boundary_;
30-
boost::shared_ptr<boost::mutex> plane_mutex_;
31-
32-
public:
33-
BoundedPlane3 () :
34-
boundary_(new Cloud()),
35-
plane_mutex_ (new boost::mutex())
36-
{}
37-
38-
// Construct from coefficients and boundary
39-
// BoundedPlane3 (double a, double b, double c, double d, CloudPtr boundary)
40-
// : OrientedPlane3 (a, b, c, d),
41-
// boundary_ (boundary)
42-
// {}
43-
44-
BoundedPlane3 (const BoundedPlane3<PointT> & plane )
45-
: n_ (plane.n_),
46-
d_ (plane.d_),
47-
boundary_ (new Cloud(*plane.boundary_)),
48-
plane_mutex_ (plane.plane_mutex_)
49-
{
50-
//boost::lock_guard<boost::mutex> lock (*plane_mutex_);
51-
//boundary_ = plane.boundary_;
52-
}
53-
54-
BoundedPlane3 (const gtsam::Unit3& s, double d, CloudPtr boundary, MutexPtr boundary_mutex)
55-
: n_ (s),
56-
d_ (d),
57-
boundary_ (new Cloud(*boundary)),
58-
plane_mutex_(boundary_mutex)
59-
{
60-
//boost::lock_guard<boost::mutex> lock (*plane_mutex_);
61-
//boundary_ = boundary;
62-
}
63-
64-
BoundedPlane3 (double a, double b, double c, double d, CloudPtr boundary, MutexPtr boundary_mutex = MutexPtr(new boost::mutex()))
65-
: n_ (gtsam::Unit3(gtsam::Point3(a, b, c))),
66-
d_ (d),
67-
boundary_ (new Cloud(*boundary)),
68-
plane_mutex_ (boundary_mutex)
69-
{
70-
//boost::lock_guard<boost::mutex> lock (*plane_mutex_);
71-
//boundary_ = boundary;
72-
//gtsam::Point3 p (a, b, c);
73-
//n_ = gtsam::Unit3 (p);
74-
//d_ = d;
75-
//boundary_ = boundary;
76-
}
77-
78-
/// The print fuction
79-
void print(const std::string& s = std::string()) const;
80-
81-
/// The equals function with tolerance
82-
bool equals(const BoundedPlane3<PointT>& s, double tol = 1e-9) const {
83-
return (n_.equals(s.n_, tol) && (fabs (d_ - s.d_) < tol));
84-
}
85-
86-
// Construct from a measurement at some pose, placing the measurement in the map frame
87-
//BoundedPlane3 (const gtsam::Pose3& pose, BoundedPlane3& plane_measurement);
88-
89-
/// Computes the error between two poses
90-
gtsam::Vector error (const BoundedPlane3<PointT>& plane) const;
91-
92-
/// Dimensionality of tangent space = 3 DOF
93-
inline static size_t Dim() {
94-
return 3;
95-
}
96-
97-
/// Dimensionality of tangent space = 3 DOF
98-
inline size_t dim() const {
99-
return 3;
100-
}
101-
102-
/// Returns the plane coefficients (a, b, c, d)
103-
gtsam::Vector planeCoefficients () const;
104-
105-
inline gtsam::Unit3 normal () const {
106-
return n_;
107-
}
108-
109-
// Override Plane3 retract to additionally reproject the boundary when needed
110-
BoundedPlane3 retract (const gtsam::Vector& v) const;
111-
112-
/// The local coordinates function
113-
gtsam::Vector localCoordinates(const BoundedPlane3<PointT>& s) const;
114-
115-
// reprojects the boundary to the current plane coefficients
116-
void reprojectBoundary ();
117-
118-
// extend the boundary cloud with a new measurement
119-
void extendBoundary (const gtsam::Pose3& pose, BoundedPlane3<PointT>& plane) const;
120-
121-
// retract the boundary cloud to a given measurement
122-
void retractBoundary (const gtsam::Pose3& pose, BoundedPlane3<PointT>& plane);
123-
124-
CloudPtr boundary () const { return (boundary_); }
125-
126-
double d () { return (d_); }
127-
128-
static BoundedPlane3 Transform (const omnimapper::BoundedPlane3<PointT>& plane,
129-
const gtsam::Pose3& xr,
130-
boost::optional<gtsam::Matrix&> Hr,
131-
boost::optional<gtsam::Matrix&> Hp);
132-
133-
static Eigen::Vector4d TransformCoefficients (const omnimapper::BoundedPlane3<PointT>& plane,
134-
const gtsam::Pose3& xr);
135-
136-
};
137-
138-
}
12+
namespace omnimapper {
13+
/**
14+
* A planar landmark bounded by a polygonal point cloud.
15+
*/
16+
template <typename PointT>
17+
class BoundedPlane3 : public gtsam::DerivedValue<BoundedPlane3<PointT> > {
18+
typedef typename pcl::PointCloud<PointT> Cloud;
19+
typedef typename Cloud::Ptr CloudPtr;
20+
typedef typename Cloud::ConstPtr CloudConstPtr;
21+
typedef typename boost::shared_ptr<boost::mutex> MutexPtr;
22+
23+
protected:
24+
gtsam::Unit3 n_;
25+
double d_;
26+
27+
CloudPtr boundary_;
28+
boost::shared_ptr<boost::mutex> plane_mutex_;
29+
30+
public:
31+
BoundedPlane3() : boundary_(new Cloud()), plane_mutex_(new boost::mutex()) {}
32+
33+
// Construct from coefficients and boundary
34+
// BoundedPlane3 (double a, double b, double c, double d, CloudPtr boundary)
35+
// : OrientedPlane3 (a, b, c, d),
36+
// boundary_ (boundary)
37+
// {}
38+
39+
BoundedPlane3(const BoundedPlane3<PointT>& plane)
40+
: n_(plane.n_),
41+
d_(plane.d_),
42+
boundary_(new Cloud(*plane.boundary_)),
43+
plane_mutex_(plane.plane_mutex_) {
44+
// boost::lock_guard<boost::mutex> lock (*plane_mutex_);
45+
// boundary_ = plane.boundary_;
46+
}
47+
48+
BoundedPlane3(const gtsam::Unit3& s, double d, CloudPtr boundary,
49+
MutexPtr boundary_mutex)
50+
: n_(s),
51+
d_(d),
52+
boundary_(new Cloud(*boundary)),
53+
plane_mutex_(boundary_mutex) {
54+
// boost::lock_guard<boost::mutex> lock (*plane_mutex_);
55+
// boundary_ = boundary;
56+
}
57+
58+
BoundedPlane3(double a, double b, double c, double d, CloudPtr boundary,
59+
MutexPtr boundary_mutex = MutexPtr(new boost::mutex()))
60+
: n_(gtsam::Unit3(gtsam::Point3(a, b, c))),
61+
d_(d),
62+
boundary_(new Cloud(*boundary)),
63+
plane_mutex_(boundary_mutex) {
64+
// boost::lock_guard<boost::mutex> lock (*plane_mutex_);
65+
// boundary_ = boundary;
66+
// gtsam::Point3 p (a, b, c);
67+
// n_ = gtsam::Unit3 (p);
68+
// d_ = d;
69+
// boundary_ = boundary;
70+
}
71+
72+
/// The print fuction
73+
void print(const std::string& s = std::string()) const;
74+
75+
/// The equals function with tolerance
76+
bool equals(const BoundedPlane3<PointT>& s, double tol = 1e-9) const {
77+
return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol));
78+
}
79+
80+
// Construct from a measurement at some pose, placing the measurement in the
81+
// map frame
82+
// BoundedPlane3 (const gtsam::Pose3& pose, BoundedPlane3& plane_measurement);
83+
84+
/// Computes the error between two poses
85+
gtsam::Vector error(const BoundedPlane3<PointT>& plane) const;
86+
87+
/// Dimensionality of tangent space = 3 DOF
88+
inline static size_t Dim() { return 3; }
89+
90+
/// Dimensionality of tangent space = 3 DOF
91+
inline size_t dim() const { return 3; }
92+
93+
/// Returns the plane coefficients (a, b, c, d)
94+
gtsam::Vector planeCoefficients() const;
95+
96+
inline gtsam::Unit3 normal() const { return n_; }
97+
98+
// Override Plane3 retract to additionally reproject the boundary when needed
99+
BoundedPlane3 retract(const gtsam::Vector& v) const;
100+
101+
/// The local coordinates function
102+
gtsam::Vector localCoordinates(const BoundedPlane3<PointT>& s) const;
103+
104+
// reprojects the boundary to the current plane coefficients
105+
void reprojectBoundary();
106+
107+
// extend the boundary cloud with a new measurement
108+
void extendBoundary(const gtsam::Pose3& pose,
109+
BoundedPlane3<PointT>& plane) const;
110+
111+
// retract the boundary cloud to a given measurement
112+
void retractBoundary(const gtsam::Pose3& pose, BoundedPlane3<PointT>& plane);
113+
114+
CloudPtr boundary() const { return (boundary_); }
115+
116+
double d() { return (d_); }
117+
118+
static BoundedPlane3 Transform(const omnimapper::BoundedPlane3<PointT>& plane,
119+
const gtsam::Pose3& xr,
120+
boost::optional<gtsam::Matrix&> Hr,
121+
boost::optional<gtsam::Matrix&> Hp);
122+
123+
static Eigen::Vector4d TransformCoefficients(
124+
const omnimapper::BoundedPlane3<PointT>& plane, const gtsam::Pose3& xr);
125+
};
126+
127+
} // namespace omnimapper

‎include/omnimapper/BoundedPlaneFactor.h

Lines changed: 42 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -1,48 +1,47 @@
11
#pragma once
22

3-
#include <gtsam/slam/OrientedPlane3Factor.h>
43
#include <gtsam/linear/NoiseModel.h>
4+
#include <gtsam/slam/OrientedPlane3Factor.h>
55
#include <omnimapper/BoundedPlane3.h>
66

7-
namespace omnimapper
8-
{
9-
template <typename PointT>
10-
class BoundedPlaneFactor : public gtsam::NoiseModelFactor2<gtsam::Pose3, omnimapper::BoundedPlane3<PointT> >::NoiseModelFactor2
11-
{
12-
typedef typename pcl::PointCloud<PointT> Cloud;
13-
typedef typename Cloud::Ptr CloudPtr;
14-
typedef typename Cloud::ConstPtr CloudConstPtr;
15-
16-
protected:
17-
//using gtsam::OrientedPlane3Factor::poseKey_;
18-
//using gtsam::OrientedPlane3Factor::landmarkKey_;
19-
gtsam::Key poseKey_;
20-
gtsam::Key landmarkKey_;
21-
BoundedPlane3<PointT> measured_p_;
22-
23-
typedef gtsam::NoiseModelFactor2<gtsam::Pose3, BoundedPlane3<PointT> > Base;
24-
25-
public:
26-
27-
BoundedPlaneFactor ()
28-
{}
29-
30-
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
31-
BoundedPlaneFactor (const gtsam::Vector&z, CloudPtr boundary, const gtsam::SharedGaussian& noiseModel,
32-
gtsam::Key pose,
33-
gtsam::Key landmark)
34-
: Base (noiseModel, pose, landmark),
35-
poseKey_ (pose),
36-
landmarkKey_ (landmark)
37-
{
38-
measured_p_ = BoundedPlane3<PointT> (z (0), z (1), z (2), z (3), boundary);
39-
}
40-
41-
/// print
42-
void print(const std::string& s="BoundedPlaneFactor") const;
43-
44-
virtual gtsam::Vector evaluateError(const gtsam::Pose3& pose, const BoundedPlane3<PointT>& plane,
45-
boost::optional<gtsam::Matrix&> H1 = boost::none,
46-
boost::optional<gtsam::Matrix&> H2 = boost::none) const;
47-
};
48-
}
7+
namespace omnimapper {
8+
template <typename PointT>
9+
class BoundedPlaneFactor
10+
: public gtsam::NoiseModelFactor2<
11+
gtsam::Pose3, omnimapper::BoundedPlane3<PointT> >::NoiseModelFactor2 {
12+
typedef typename pcl::PointCloud<PointT> Cloud;
13+
typedef typename Cloud::Ptr CloudPtr;
14+
typedef typename Cloud::ConstPtr CloudConstPtr;
15+
16+
protected:
17+
// using gtsam::OrientedPlane3Factor::poseKey_;
18+
// using gtsam::OrientedPlane3Factor::landmarkKey_;
19+
gtsam::Key poseKey_;
20+
gtsam::Key landmarkKey_;
21+
BoundedPlane3<PointT> measured_p_;
22+
23+
typedef gtsam::NoiseModelFactor2<gtsam::Pose3, BoundedPlane3<PointT> > Base;
24+
25+
public:
26+
BoundedPlaneFactor() {}
27+
28+
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose
29+
/// symbol
30+
BoundedPlaneFactor(const gtsam::Vector& z, CloudPtr boundary,
31+
const gtsam::SharedGaussian& noiseModel, gtsam::Key pose,
32+
gtsam::Key landmark)
33+
: Base(noiseModel, pose, landmark),
34+
poseKey_(pose),
35+
landmarkKey_(landmark) {
36+
measured_p_ = BoundedPlane3<PointT>(z(0), z(1), z(2), z(3), boundary);
37+
}
38+
39+
/// print
40+
void print(const std::string& s = "BoundedPlaneFactor") const;
41+
42+
virtual gtsam::Vector evaluateError(
43+
const gtsam::Pose3& pose, const BoundedPlane3<PointT>& plane,
44+
boost::optional<gtsam::Matrix&> H1 = boost::none,
45+
boost::optional<gtsam::Matrix&> H2 = boost::none) const;
46+
};
47+
} // namespace omnimapper

0 commit comments

Comments
 (0)