点云法向量估计方法

已经大半年没更新博客了，今天来更新一下博客。法线在点云里作为表述点云特征的一种重要信息，pcl提供了成熟的接口。当然自己去实现，算法难度也不大，为了熟悉pcl源码，博主之前对pcl源码求取法线的代码进行了剥离，同时对算法的实现流程做了一个简答的表述，让法线从此不再神秘。

点云法向量的估计流程

1.对于第一步就比较常规了，建议大家直接使用pcl的kdtree或者octree进行最近邻搜索

2.求协方差矩阵，这里贴一段代码（当然也可以自己实现，难度不大）

//协方差矩阵求解方法
unsigned int
computeMeanAndCovarianceMatrix(const pcl::PointCloud<PointXYZ> &cloud,
Eigen::Matrix<double, 3, 3> &covariance_matrix,
Eigen::Matrix<double, 4, 1> &centroid)
{
    // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
    Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero();
    size_t point_count;
    if (cloud.is_dense)
    {
        point_count = cloud.size();
        // For each point in the cloud
        for (size_t i = 0; i < point_count; ++i)
        {
            accu[0] += cloud[i].x * cloud[i].x;
            accu[1] += cloud[i].x * cloud[i].y;
            accu[2] += cloud[i].x * cloud[i].z;
            accu[3] += cloud[i].y * cloud[i].y; // 4
            accu[4] += cloud[i].y * cloud[i].z; // 5
            accu[5] += cloud[i].z * cloud[i].z; // 8
            accu[6] += cloud[i].x;
            accu[7] += cloud[i].y;
            accu[8] += cloud[i].z;
        }
    }
    else
    {
        point_count = 0;
        for (size_t i = 0; i < cloud.size(); ++i)
        {
            if (!isFinite(cloud[i]))
                continue;

            accu[0] += cloud[i].x * cloud[i].x;
            accu[1] += cloud[i].x * cloud[i].y;
            accu[2] += cloud[i].x * cloud[i].z;
            accu[3] += cloud[i].y * cloud[i].y;
            accu[4] += cloud[i].y * cloud[i].z;
            accu[5] += cloud[i].z * cloud[i].z;
            accu[6] += cloud[i].x;
            accu[7] += cloud[i].y;
            accu[8] += cloud[i].z;
            ++point_count;
        }
    }
    accu /= static_cast<double> (point_count);
    if (point_count != 0)
    {
        //centroid.head<3> () = accu.tail<3> ();    -- does not compile with Clang 3.0
        centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
        centroid[3] = 1;
        covariance_matrix.coeffRef(0) = accu[0] - accu[6] * accu[6];
        covariance_matrix.coeffRef(1) = accu[1] - accu[6] * accu[7];
        covariance_matrix.coeffRef(2) = accu[2] - accu[6] * accu[8];
        covariance_matrix.coeffRef(4) = accu[3] - accu[7] * accu[7];
        covariance_matrix.coeffRef(5) = accu[4] - accu[7] * accu[8];
        covariance_matrix.coeffRef(8) = accu[5] - accu[8] * accu[8];
        covariance_matrix.coeffRef(3) = covariance_matrix.coeff(1);
        covariance_matrix.coeffRef(6) = covariance_matrix.coeff(2);
        covariance_matrix.coeffRef(7) = covariance_matrix.coeff(5);
    }
    return (static_cast<unsigned int> (point_count));
}

3.特征值以及对应的特征向量的求解（贴一段代码）

说明一下eigen也提供了相关的方法，博主这里对pcl的源码进行了整理，二者所求结果不尽一致，孰优孰劣大家可以在日常使用中去进行检验。

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
//#include <pcl/common/centroid.h>
//#include <pcl/common/eigen.h>
using namespace Eigen;
using namespace std;
//using namespace pcl;
 
 
 void computeRoots2(const Matrix3d::Scalar& b, const Matrix3d::Scalar& c, Eigen::Vector3d& roots)
{
     roots(0) = Matrix3d::Scalar(0);
     Matrix3d::Scalar d = Matrix3d::Scalar(b * b - 4.0 * c);
    if (d < 0.0)  // no real roots ! THIS SHOULD NOT HAPPEN!
        d = 0.0;
 
    Matrix3d::Scalar sd = ::std::sqrt(d);
 
    roots(2) = 0.5f * (b + sd);
    roots(1) = 0.5f * (b - sd);
}
 
void computeRoots(const Matrix3d& m, Eigen::Vector3d& roots)
{
    typedef  Matrix3d::Scalar Scalar;
 
    // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
    // eigenvalues are the roots to this equation, all guaranteed to be
    // real-valued, because the matrix is symmetric.
    Scalar c0 = m(0, 0) * m(1, 1) * m(2, 2)
        + Scalar(2) * m(0, 1) * m(0, 2) * m(1, 2)
        - m(0, 0) * m(1, 2) * m(1, 2)
        - m(1, 1) * m(0, 2) * m(0, 2)
        - m(2, 2) * m(0, 1) * m(0, 1);
    Scalar c1 = m(0, 0) * m(1, 1) -
        m(0, 1) * m(0, 1) +
        m(0, 0) * m(2, 2) -
        m(0, 2) * m(0, 2) +
        m(1, 1) * m(2, 2) -
        m(1, 2) * m(1, 2);
    Scalar c2 = m(0, 0) + m(1, 1) + m(2, 2);
 
    if (fabs(c0) < Eigen::NumTraits < Scalar > ::epsilon())  // one root is 0 -> quadratic equation
        computeRoots2(c2, c1, roots);
    else
    {
        const Scalar s_inv3 = Scalar(1.0 / 3.0);
        const Scalar s_sqrt3 = std::sqrt(Scalar(3.0));
        // Construct the parameters used in classifying the roots of the equation
        // and in solving the equation for the roots in closed form.
        Scalar c2_over_3 = c2 * s_inv3;
        Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
        if (a_over_3 > Scalar(0))
            a_over_3 = Scalar(0);
 
        Scalar half_b = Scalar(0.5) * (c0 + c2_over_3 * (Scalar(2) * c2_over_3 * c2_over_3 - c1));
 
        Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
        if (q > Scalar(0))
            q = Scalar(0);
 
        // Compute the eigenvalues by solving for the roots of the polynomial.
        Scalar rho = std::sqrt(-a_over_3);
        Scalar theta = std::atan2(std::sqrt(-q), half_b) * s_inv3;
        Scalar cos_theta = std::cos(theta);
        Scalar sin_theta = std::sin(theta);
        roots(0) = c2_over_3 + Scalar(2) * rho * cos_theta;
        roots(1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
        roots(2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
 
        // Sort in increasing order.
        if (roots(0) >= roots(1))
            std::swap(roots(0), roots(1));
        if (roots(1) >= roots(2))
        {
            std::swap(roots(1), roots(2));
            if (roots(0) >= roots(1))
                std::swap(roots(0), roots(1));
        }
 
        if (roots(0) <= 0)  // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
            computeRoots2(c2, c1, roots);
    }
}
 
void eigen33zx(const Matrix3d& mat, Matrix3d& evecs, Eigen::Vector3d& evals)
{
    typedef  Matrix3d::Scalar Scalar;
    // typedef  Matrix3d::Scalar Scalar;
    // Scale the matrix so its entries are in [-1,1].  The scaling is applied
    // only when at least one matrix entry has magnitude larger than 1.
 
    Scalar scale = mat.cwiseAbs().maxCoeff();
    if (scale <= std::numeric_limits < Scalar > ::min())
        scale = Scalar(1.0);
 
    Matrix3d scaledMat = mat / scale;
 
    // Compute the eigenvalues
    computeRoots(scaledMat, evals);
 
    if ((evals(2) - evals(0)) <= Eigen::NumTraits < Scalar > ::epsilon())
    {
        // all three equal
        evecs.setIdentity();
    }
    else if ((evals(1) - evals(0)) <= Eigen::NumTraits < Scalar > ::epsilon())
    {
        // first and second equal
        Matrix3d tmp;
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(2);
 
        Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
        Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
        Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));
 
        Scalar len1 = vec1.squaredNorm();
        Scalar len2 = vec2.squaredNorm();
        Scalar len3 = vec3.squaredNorm();
 
        if (len1 >= len2 && len1 >= len3)
            evecs.col(2) = vec1 / std::sqrt(len1);
        else if (len2 >= len1 && len2 >= len3)
            evecs.col(2) = vec2 / std::sqrt(len2);
        else
            evecs.col(2) = vec3 / std::sqrt(len3);
 
        evecs.col(1) = evecs.col(2).unitOrthogonal();
        evecs.col(0) = evecs.col(1).cross(evecs.col(2));
    }
    else if ((evals(2) - evals(1)) <= Eigen::NumTraits < Scalar > ::epsilon())
    {
        // second and third equal
        Matrix3d tmp;
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(0);
 
        Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
        Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
        Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));
 
        Scalar len1 = vec1.squaredNorm();
        Scalar len2 = vec2.squaredNorm();
        Scalar len3 = vec3.squaredNorm();
 
        if (len1 >= len2 && len1 >= len3)
            evecs.col(0) = vec1 / std::sqrt(len1);
        else if (len2 >= len1 && len2 >= len3)
            evecs.col(0) = vec2 / std::sqrt(len2);
        else
            evecs.col(0) = vec3 / std::sqrt(len3);
 
        evecs.col(1) = evecs.col(0).unitOrthogonal();
        evecs.col(2) = evecs.col(0).cross(evecs.col(1));
    }
    else
    {
        Matrix3d tmp;
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(2);
 
        Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
        Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
        Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));
 
        Scalar len1 = vec1.squaredNorm();
        Scalar len2 = vec2.squaredNorm();
        Scalar len3 = vec3.squaredNorm();
#ifdef _WIN32
        Scalar *mmax = new Scalar[3];
#else
        Scalar mmax[3];
#endif
        unsigned int min_el = 2;
        unsigned int max_el = 2;
        if (len1 >= len2 && len1 >= len3)
        {
            mmax[2] = len1;
            evecs.col(2) = vec1 / std::sqrt(len1);
        }
        else if (len2 >= len1 && len2 >= len3)
        {
            mmax[2] = len2;
            evecs.col(2) = vec2 / std::sqrt(len2);
        }
        else
        {
            mmax[2] = len3;
            evecs.col(2) = vec3 / std::sqrt(len3);
        }
 
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(1);
 
        vec1 = tmp.row(0).cross(tmp.row(1));
        vec2 = tmp.row(0).cross(tmp.row(2));
        vec3 = tmp.row(1).cross(tmp.row(2));
 
        len1 = vec1.squaredNorm();
        len2 = vec2.squaredNorm();
        len3 = vec3.squaredNorm();
        if (len1 >= len2 && len1 >= len3)
        {
            mmax[1] = len1;
            evecs.col(1) = vec1 / std::sqrt(len1);
            min_el = len1 <= mmax[min_el] ? 1 : min_el;
            max_el = len1 > mmax[max_el] ? 1 : max_el;
        }
        else if (len2 >= len1 && len2 >= len3)
        {
            mmax[1] = len2;
            evecs.col(1) = vec2 / std::sqrt(len2);
            min_el = len2 <= mmax[min_el] ? 1 : min_el;
            max_el = len2 > mmax[max_el] ? 1 : max_el;
        }
        else
        {
            mmax[1] = len3;
            evecs.col(1) = vec3 / std::sqrt(len3);
            min_el = len3 <= mmax[min_el] ? 1 : min_el;
            max_el = len3 > mmax[max_el] ? 1 : max_el;
        }
 
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(0);
 
        vec1 = tmp.row(0).cross(tmp.row(1));
        vec2 = tmp.row(0).cross(tmp.row(2));
        vec3 = tmp.row(1).cross(tmp.row(2));
 
        len1 = vec1.squaredNorm();
        len2 = vec2.squaredNorm();
        len3 = vec3.squaredNorm();
        if (len1 >= len2 && len1 >= len3)
        {
            mmax[0] = len1;
            evecs.col(0) = vec1 / std::sqrt(len1);
            min_el = len3 <= mmax[min_el] ? 0 : min_el;
            max_el = len3 > mmax[max_el] ? 0 : max_el;
        }
        else if (len2 >= len1 && len2 >= len3)
        {
            mmax[0] = len2;
            evecs.col(0) = vec2 / std::sqrt(len2);
            min_el = len3 <= mmax[min_el] ? 0 : min_el;
            max_el = len3 > mmax[max_el] ? 0 : max_el;
        }
        else
        {
            mmax[0] = len3;
            evecs.col(0) = vec3 / std::sqrt(len3);
            min_el = len3 <= mmax[min_el] ? 0 : min_el;
            max_el = len3 > mmax[max_el] ? 0 : max_el;
        }
 
        unsigned mid_el = 3 - min_el - max_el;
        evecs.col(min_el) = evecs.col((min_el + 1) % 3).cross(evecs.col((min_el + 2) % 3)).normalized();
        evecs.col(mid_el) = evecs.col((mid_el + 1) % 3).cross(evecs.col((mid_el + 2) % 3)).normalized();
#ifdef _WIN32
        delete[] mmax;
#endif
    }
    // Rescale back to the original size.
    evals *= scale;
}

4.根据3中的结果即可实现法向量的估计

上面的代码抽取的pcl的源码，接口较多，不过也间接说明了底层算法人员的不易，其实每一个成熟稳定的接口都来自于底层算法人员长期努力的结果，这里向那些开源的算法库致敬。

随便写的有点混乱，我还是来一个例子，对于近邻搜索这一块，网上例子铺天盖地，先略过。

Matrix3d A;
   A << 6, -3,0, -3,6, 0, 0, 0, 0;
   cout << "Here is a 3x3 matrix, A:" << endl << A << endl << endl;
   Eigen:: Vector3d eigen_values;
   // pcl::eigen33(A, eigen_values);//pcl的接口
   Matrix3d eigenVectors1;
   eigen33zx(A, eigenVectors1, eigen_values);
   cout << eigenVectors1 << endl;
   cout << eigen_values << endl;

　　上面的矩阵A是一个协方差矩阵，是博主为了检验该方法是否准确，自己随便用XoY平面上的几个点求解的。

posted @ 2019-12-28 13:53 点小二阅读(5067) 评论(4) 编辑收藏举报

刷新页面返回顶部

登录后才能查看或发表评论，立即登录或者逛逛博客园首页

阅读排行：
· 全程不用写代码，我用AI程序员写了一个飞机大战
· DeepSeek 开源周回顾「GitHub 热点速览」
· 记一次.NET内存居高不下排查解决与启示
· 物流快递公司核心技术能力-地址解析分单基础技术分享
· .NET 10首个预览版发布：重大改进与新特性概览！

点云客栈

点云法向量估计方法

公告

常用链接

随笔分类

随笔档案

阅读排行榜

评论排行榜

推荐排行榜

最新评论