41 enum class CylinderFitterType
56 CylinderFitterType fitter_ = CylinderFitterType::HemisphereSearchFit;
59 Eigen::Vector<T, 3> baseCylinderAxis_;
62 size_t thetaResolution_ = 0;
63 size_t phiResolution_ = 0;
64 bool isMultithread_ =
true;
67 std::vector<Eigen::Vector<T, 3>> normalizedPoints_ = {};
77 Eigen::Vector <T, 6> precomputedMu_ = {};
78 Eigen::Matrix <T, 3, 3> precomputedF0_ = {};
79 Eigen::Matrix <T, 3, 6> precomputedF1_ = {};
80 Eigen::Matrix <T, 6, 6> precomputedF2_ = {};
92 precomputedMu_.setZero();
93 precomputedF0_.setZero();
94 precomputedF1_.setZero();
95 precomputedF2_.setZero();
96 normalizedPoints_.clear();
102 thetaResolution_ = theta;
103 phiResolution_ = phi;
104 isMultithread_ = isMultithread;
105 fitter_ = CylinderFitterType::HemisphereSearchFit;
106 assert( thetaResolution_ > 0 );
107 assert( phiResolution_ > 0 );
108 auto result = solve( points, cylinder );
119 fitter_ = CylinderFitterType::SpecificAxisFit;
120 assert( baseCylinderAxis_.isZero() ==
false );
121 auto result = solve( points, cylinder );
129 if ( points.size() < 6 )
131 spdlog::warn(
"Cylinder3Approximation :: Too low point for cylinder approximation count={}" , points.size() );
134 assert( points.size() >= 6 );
136 normalizedPoints_.clear();
139 Eigen::Vector<T, 3> bestPC;
140 Eigen::Vector<T, 3> bestW;
145 updatePrecomputeParams( points, avgPoint );
148 if ( fitter_ == CylinderFitterType::HemisphereSearchFit )
150 if ( isMultithread_ )
151 error = fitCylindeHemisphereMultiThreaded( bestPC, bestW, rootSquare );
154 error = fitCylindeHemisphereSingleThreaded( bestPC, bestW, rootSquare );
156 else if ( fitter_ == CylinderFitterType::SpecificAxisFit )
158 error = SpecificAxisFit( bestPC, bestW, rootSquare );
162 spdlog::warn(
"Cylinder3Approximation :: unsupported fitter" );
167 assert( rootSquare >= 0 );
171 cylinder.
radius = std::sqrt( rootSquare );
175 T hmin = std::numeric_limits<T>::max();
176 T hmax = -std::numeric_limits<T>::max();
178 for (
size_t i = 0; i < points.size(); ++i )
181 hmin = std::min( h, hmin );
182 hmax = std::max( h, hmax );
184 T hmid = ( hmin + hmax ) / 2;
188 cylinder.
length = hmax - hmin;
190 assert( cylinder.
length >= 0 );
198 normalizedPoints_.resize( points.size() );
202 for (
size_t i = 0; i < points.size(); ++i )
203 average += points[i];
204 average = average /
static_cast< T
> ( points.size() );
207 for (
size_t i = 0; i < points.size(); ++i )
208 normalizedPoints_[i] =
toEigen( points[i] - average );
210 const Eigen::Vector<T, 6> zeroEigenVector6{ 0, 0, 0, 0, 0, 0 };
211 std::vector<Eigen::Vector<T, 6>> products( normalizedPoints_.size(), zeroEigenVector6 );
212 precomputedMu_ = zeroEigenVector6;
214 for (
size_t i = 0; i < normalizedPoints_.size(); ++i )
216 products[i][0] = normalizedPoints_[i][0] * normalizedPoints_[i][0];
217 products[i][1] = normalizedPoints_[i][0] * normalizedPoints_[i][1];
218 products[i][2] = normalizedPoints_[i][0] * normalizedPoints_[i][2];
219 products[i][3] = normalizedPoints_[i][1] * normalizedPoints_[i][1];
220 products[i][4] = normalizedPoints_[i][1] * normalizedPoints_[i][2];
221 products[i][5] = normalizedPoints_[i][2] * normalizedPoints_[i][2];
222 precomputedMu_[0] += products[i][0];
223 precomputedMu_[1] += 2 * products[i][1];
224 precomputedMu_[2] += 2 * products[i][2];
225 precomputedMu_[3] += products[i][3];
226 precomputedMu_[4] += 2 * products[i][4];
227 precomputedMu_[5] += products[i][5];
229 precomputedMu_ = precomputedMu_ / points.size();
231 precomputedF0_.setZero();
232 precomputedF1_.setZero();
233 precomputedF2_.setZero();
234 for (
size_t i = 0; i < normalizedPoints_.size(); ++i )
236 Eigen::Vector<T, 6> delta{};
237 delta[0] = products[i][0] - precomputedMu_[0];
238 delta[1] = 2 * products[i][1] - precomputedMu_[1];
239 delta[2] = 2 * products[i][2] - precomputedMu_[2];
240 delta[3] = products[i][3] - precomputedMu_[3];
241 delta[4] = 2 * products[i][4] - precomputedMu_[4];
242 delta[5] = products[i][5] - precomputedMu_[5];
243 precomputedF0_( 0, 0 ) += products[i][0];
244 precomputedF0_( 0, 1 ) += products[i][1];
245 precomputedF0_( 0, 2 ) += products[i][2];
246 precomputedF0_( 1, 1 ) += products[i][3];
247 precomputedF0_( 1, 2 ) += products[i][4];
248 precomputedF0_( 2, 2 ) += products[i][5];
249 precomputedF1_ = precomputedF1_ + normalizedPoints_[i] * delta.transpose();
250 precomputedF2_ += delta * delta.transpose();
252 precomputedF0_ = precomputedF0_ /
static_cast < T
> ( points.size() );
253 precomputedF0_( 1, 0 ) = precomputedF0_( 0, 1 );
254 precomputedF0_( 2, 0 ) = precomputedF0_( 0, 2 );
255 precomputedF0_( 2, 1 ) = precomputedF0_( 1, 2 );
256 precomputedF1_ = precomputedF1_ /
static_cast < T
> ( points.size() );
257 precomputedF2_ = precomputedF2_ /
static_cast < T
> ( points.size() );
264 T G(
const Eigen::Vector<T, 3>& W, Eigen::Vector<T, 3>& PC, T& rsqr )
const
266 Eigen::Matrix<T, 3, 3> P = Eigen::Matrix<T, 3, 3>::Identity() - ( W * W.transpose() );
268 Eigen::Matrix<T, 3, 3> S;
273 Eigen::Matrix<T, 3, 3>
A = P * precomputedF0_ * P;
274 Eigen::Matrix<T, 3, 3> hatA = -( S *
A * S );
275 Eigen::Matrix<T, 3, 3> hatAA = hatA *
A;
276 T trace = hatAA.trace();
277 Eigen::Matrix<T, 3, 3> Q = hatA / trace;
278 Eigen::Vector<T, 6> pVec{ P( 0, 0 ), P( 0, 1 ), P( 0, 2 ), P( 1, 1 ), P( 1, 2 ), P( 2, 2 ) };
279 Eigen::Vector<T, 3> alpha = precomputedF1_ * pVec;
280 Eigen::Vector<T, 3> beta = Q * alpha;
281 T error = ( pVec.dot( precomputedF2_ * pVec ) - 4 * alpha.dot( beta ) + 4 * beta.dot( precomputedF0_ * beta ) ) /
static_cast< T
>( normalizedPoints_.size() );
286 error = std::abs( error );
289 rsqr = pVec.dot( precomputedMu_ ) + beta.dot( beta );
294 T fitCylindeHemisphereSingleThreaded( Eigen::Vector<T, 3>& PC, Eigen::Vector<T, 3>& W, T& resultedRootSquare )
const
296 T
const theraStep =
static_cast< T
>( 2 * PI ) / thetaResolution_;
297 T
const phiStep =
static_cast< T
>( PI2 ) / phiResolution_;
301 T minError = G( W, PC, resultedRootSquare );
303 for (
size_t j = 1; j <= phiResolution_; ++j )
306 T cosPhi = std::cos( phi );
307 T sinPhi = std::sin( phi );
308 for (
size_t i = 0; i < thetaResolution_; ++i )
310 T theta = theraStep * i;
311 T cosTheta = std::cos( theta );
312 T sinTheta = std::sin( theta );
313 Eigen::Vector<T, 3> currW{ cosTheta * sinPhi, sinTheta * sinPhi, cosPhi };
314 Eigen::Vector<T, 3> currPC{};
316 T error = G( currW, currPC, rsqr );
317 if ( error < minError )
320 resultedRootSquare = rsqr;
330 class BestHemisphereStoredData
333 T error = std::numeric_limits<T>::max();
334 T rootSquare = std::numeric_limits<T>::max();
335 Eigen::Vector<T, 3> W;
336 Eigen::Vector<T, 3> PC;
339 T fitCylindeHemisphereMultiThreaded( Eigen::Vector<T, 3>& PC, Eigen::Vector<T, 3>& W, T& resultedRootSquare )
const
341 T
const theraStep =
static_cast< T
>( 2 * PI ) / thetaResolution_;
342 T
const phiStep =
static_cast< T
>( PI2 ) / phiResolution_;
346 T minError = G( W, PC, resultedRootSquare );
348 std::vector<BestHemisphereStoredData> storedData;
349 storedData.resize( phiResolution_ + 1 );
351 tbb::parallel_for( tbb::blocked_range<size_t>(
size_t( 0 ), phiResolution_ + 1 ),
352 [&] (
const tbb::blocked_range<size_t>& range )
354 for (
size_t j = range.begin(); j < range.end(); ++j )
358 T cosPhi = std::cos( phi );
359 T sinPhi = std::sin( phi );
360 for (
size_t i = 0; i < thetaResolution_; ++i )
363 T theta = theraStep * i;
364 T cosTheta = std::cos( theta );
365 T sinTheta = std::sin( theta );
366 Eigen::Vector<T, 3> currW{ cosTheta * sinPhi, sinTheta * sinPhi, cosPhi };
367 Eigen::Vector<T, 3> currPC{};
369 T error = G( currW, currPC, rsqr );
371 if ( error < storedData[j].error )
373 storedData[j].error = error;
374 storedData[j].rootSquare = rsqr;
375 storedData[j].W = currW;
376 storedData[j].PC = currPC;
383 for (
size_t i = 0; i <= phiResolution_; ++i )
385 if ( storedData[i].error < minError )
387 minError = storedData[i].error;
388 resultedRootSquare = storedData[i].rootSquare;
390 PC = storedData[i].PC;
397 T SpecificAxisFit( Eigen::Vector<T, 3>& PC, Eigen::Vector<T, 3>& W, T& resultedRootSquare )
399 W = baseCylinderAxis_;
400 return G( W, PC, resultedRootSquare );