58 pcl::PointIndices::Ptr inliers=boost::make_shared<pcl::PointIndices>();
60 pcl::SACSegmentation<ARPoint> seg;
61 seg.setOptimizeCoefficients(
true);
62 seg.setModelType(pcl::SACMODEL_PLANE);
63 seg.setMethodType(pcl::SAC_RANSAC);
64 seg.setDistanceThreshold(distance_threshold_);
66 seg.setInputCloud(cloud);
67 seg.segment(*inliers, res.
coeffs);
69 pcl::ExtractIndices<ARPoint> extracter;
70 extracter.setInputCloud(cloud);
71 extracter.setIndices(inliers);
72 extracter.setNegative(
false);
78 ARCloud::Ptr
filterCloud (
const ARCloud& cloud,
const vector<cv::Point, Eigen::aligned_allocator<cv::Point> >& pixels)
80 ARCloud::Ptr out(
new ARCloud());
83 for(
size_t i=0; i<pixels.size(); i++)
85 const cv::Point& p = pixels[i];
86 const ARPoint& pt = cloud(p.x, p.y);
87 if (std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z)){
91 out->points.push_back(pt);
103 for(
size_t i=0; i<points.size(); i++)
105 sum.x += points[i].x;
106 sum.y += points[i].y;
107 sum.z += points[i].z;
111 const size_t n = points.size();
131 int getCoeffs (
const pcl::ModelCoefficients& coeffs,
double*
a,
double* b,
132 double* c,
double*
d)
134 if(coeffs.values.size() != 4)
136 const double s = coeffs.values[0]*coeffs.values[0] +
137 coeffs.values[1]*coeffs.values[1] + coeffs.values[2]*coeffs.values[2];
140 *a = coeffs.values[0]/s;
141 *b = coeffs.values[1]/s;
142 *c = coeffs.values[2]/s;
143 *d = coeffs.values[3]/s;
149 const double c,
const double d)
151 const double t = a*p.
x + b*p.y + c*p.z +
d;
157 str <<
"[" << m[0][0] <<
", " << m[0][1] <<
", " << m[0][2] <<
"; " 158 << m[1][0] <<
", " << m[1][1] <<
", " << m[1][2] <<
"; " 159 << m[2][0] <<
", " << m[2][1] <<
", " << m[2][2] <<
"]";
165 str <<
"[(" << q.x() <<
", " << q.y() <<
", " << q.z() <<
166 "), " << q.w() <<
"]";
172 str <<
"(" << v.
x() <<
", " << v.
y() <<
", " << v.
z() <<
")";
182 double a=0, b=0, c=0,
d=0;
192 if((q2-q1).
length() < 1e-3)
199 tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
229 Eigen::Matrix3f eig_m;
230 for(
int i=0; i<3; i++){
231 for(
int j=0; j<3; j++){
232 eig_m(i,j) = m[i][j];
235 Eigen::Quaternion<float> eig_quat(eig_m);
252 gm::Quaternion &retQ)
pcl::PointCloud< ARPoint > ARCloud
int extractOrientation(const pcl::ModelCoefficients &coeffs, const ARPoint &p1, const ARPoint &p2, const ARPoint &p3, const ARPoint &p4, geometry_msgs::Quaternion &retQ)
ARCloud::Ptr filterCloud(const ARCloud &cloud, const std::vector< cv::Point, Eigen::aligned_allocator< cv::Point > > &pixels)
Matrix3x3 inverse() const
int getQuaternion(const tf::Matrix3x3 &m, tf::Quaternion &retQ)
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE Vector3 normalized() const
TFSIMD_FORCE_INLINE tfScalar dot(const Vector3 &v) const
TFSIMD_FORCE_INLINE const tfScalar & z() const
int getCoeffs(const pcl::ModelCoefficients &coeffs, double *a, double *b, double *c, double *d)
tf::Vector3 project(const ARPoint &p, const double a, const double b, const double c, const double d)
geometry_msgs::Point centroid(const ARCloud &points)
ostream & operator<<(ostream &str, const tf::Matrix3x3 &m)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
gm::Quaternion makeQuaternion(double x, double y, double z, double w)
const double distance_threshold_
tfScalar determinant() const
int extractFrame(const pcl::ModelCoefficients &coeffs, const ARPoint &p1, const ARPoint &p2, const ARPoint &p3, const ARPoint &p4, tf::Matrix3x3 &retmat)
pcl::ModelCoefficients coeffs
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
PlaneFitResult fitPlane(ARCloud::ConstPtr cloud)
Quaternion normalized() const
int ALVAR_EXPORT diff(const std::vector< C > &v, std::vector< C > &ret)
Calculates the difference between the consecutive vector elements.