170 return solveSpecificAxisFit_( points, cone );
173 return solveHemisphereSearchFit_( points, cone );
176 return solveApproximationPCM_( points, cone );
179 return std::numeric_limits<T>::max();
188 Cone3ApproximationParams params_;
192 Cone3<T>& cone,
bool useConeInputAsInitialGuess =
false )
195 ConeFittingFunctor<T> coneFittingFunctor;
196 coneFittingFunctor.setPoints( points );
197 Eigen::LevenbergMarquardt<ConeFittingFunctor<T>, T> lm( coneFittingFunctor );
198 lm.parameters.maxfev = params_.levenbergMarquardtMaxIteration;
201 computeCenterAndNormal_( points, center, U );
204 if ( useConeInputAsInitialGuess )
210 cone = computeInitialCone_( points, center, U );
213 Eigen::VectorX<T> fittedParams( 6 );
214 coneToFitParams_( cone, fittedParams );
215 [[maybe_unused]] Eigen::LevenbergMarquardtSpace::Status result = lm.minimize( fittedParams );
220 fitParamsToCone_( fittedParams, cone );
222 T
const one_v =
static_cast< T
>( 1 );
223 auto cosAngle = std::clamp( one_v / coneAxis.
length(),
static_cast< T
>( 0 ), one_v );
224 cone.angle = std::acos( cosAngle );
225 cone.direction() = cone.direction().normalized();
226 cone.height = calculateConeHeight_( points, cone );
228 return getApproximationRMS_( points, cone );
233 return solveFixedAxis_( points, cone,
false );
238 return solveFixedAxis_( points, cone,
true );
245 ConeFittingFunctor<T> coneFittingFunctor;
246 coneFittingFunctor.setPoints( points );
248 constexpr T pi2 =
static_cast< T
>( PI2 );
249 T
const theraStep =
static_cast< T
>( 2 * PI ) / params_.hemisphereSearchPhiResolution;
250 T
const phiStep = pi2 / params_.hemisphereSearchPhiResolution;
254 T minError = std::numeric_limits<T> ::max();
256 std::vector<BestCone> bestCones;
257 bestCones.resize( params_.hemisphereSearchPhiResolution + 1 );
259 tbb::parallel_for( tbb::blocked_range<size_t>(
size_t( 0 ), params_.hemisphereSearchPhiResolution + 1 ),
260 [&] (
const tbb::blocked_range<size_t>& range )
262 for ( size_t j = range.begin(); j < range.end(); ++j )
265 T cosPhi = std::cos( phi );
266 T sinPhi = std::sin( phi );
267 for ( size_t i = 0; i < params_.hemisphereSearchThetaResolution; ++i )
269 T theta = theraStep * i;
270 T cosTheta = std::cos( theta );
271 T sinTheta = std::sin( theta );
274 Vector3<T> U( cosTheta * sinPhi, sinTheta * sinPhi, cosPhi );
276 auto tmpCone = computeInitialCone_( points, center, U );
278 Eigen::VectorX<T> fittedParams( 6 );
279 coneToFitParams_( tmpCone, fittedParams );
282 Eigen::LevenbergMarquardt<ConeFittingFunctor<T>, T> lm( coneFittingFunctor );
283 lm.parameters.maxfev = params_.levenbergMarquardtMaxIteration;
284 [[maybe_unused]] Eigen::LevenbergMarquardtSpace::Status result = lm.minimize( fittedParams );
289 fitParamsToCone_( fittedParams, tmpCone );
291 T const one_v = static_cast< T >( 1 );
292 auto cosAngle = std::clamp( one_v / tmpCone.direction().length(), static_cast< T >( 0 ), one_v );
293 tmpCone.angle = std::acos( cosAngle );
294 tmpCone.direction() = tmpCone.direction().normalized();
297 T error = getApproximationRMS_( points, tmpCone );
298 if ( error < bestCones[j].minError )
300 bestCones[j].minError = error;
301 bestCones[j].bestCone = tmpCone;
308 auto bestAppox = std::min_element( bestCones.begin(), bestCones.end(), [] (
const BestCone& a,
const BestCone& b )
310 return a.minError < b.minError;
313 cone = bestAppox->bestCone;
316 cone.height = calculateConeHeight_( points, cone );
318 return bestAppox->minError;
324 T
length =
static_cast< T
> ( 0 );
325 for (
auto i = 0; i < points.size(); ++i )
327 length = std::max(
length, std::abs( MR::dot( points[i] - cone.apex(), cone.direction() ) ) );
334 if ( points.size() == 0 )
335 return std::numeric_limits<T>::max();
338 for (
auto p : points )
339 error = error + ( cone.projectPoint( p ) - p ).
lengthSq();
341 return error / points.size();
348 for (
auto i = 0; i < points.size(); ++i )
352 center = center /
static_cast< T
>( points.size() );
359 center = computeCenter_( points );
363 for (
auto i = 0; i < points.size(); ++i )
366 U +=
Z * MR::dot(
Z,
Z );
377 result.direction() = axis;
379 T& coneAngle = result.
angle;
386 std::vector<Vector2<T>> hrPairs( points.size() );
387 T hMin = std::numeric_limits<T>::max(), hMax = -hMin;
388 for (
auto i = 0; i < points.size(); ++i )
391 T h = MR::dot( U, delta );
392 hMin = std::min( hMin, h );
393 hMax = std::max( hMax, h );
394 Vector3<T> projection = delta - MR::dot( U, delta ) * U;
395 T r = projection.
length();
396 hrPairs[i] = { h, r };
404 findBestFitLine_( hrPairs, a, b, &avgPoint );
405 T hAverage = avgPoint.
x;
406 T rAverage = avgPoint.
y;
416 std::swap( hMin, hMax );
422 T rMin = rAverage + hrSlope * ( hMin - hAverage );
423 T rMax = rAverage + hrSlope * ( hMax - hAverage );
424 T hRange = hMax - hMin;
425 T rRange = rMax - rMin;
428 T tanAngle = rRange / hRange;
429 coneAngle = std::atan2( rRange, hRange );
432 T offset = rMax / tanAngle - hMax;
433 coneApex = center - offset * U;
440 auto numPoints = xyPairs.size();
441 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
A( numPoints, 2 );
442 Eigen::Vector<T, Eigen::Dynamic> b( numPoints );
444 for (
auto i = 0; i < numPoints; ++i )
446 A( i, 0 ) = xyPairs[i].x;
448 b( i ) = xyPairs[i].y;
450 *avg = *avg + xyPairs[i];
454 *avg = *avg /
static_cast < T
> ( xyPairs.size() );
457 Eigen::Matrix<T, Eigen::Dynamic, 1> x =
A.bdcSvd( Eigen::ComputeThinU | Eigen::ComputeThinV ).solve( b );
464 *avg = *avg /
static_cast < T
> ( xyPairs.size() );
465 avg->y = lineA * avg->x + lineB;
470 void fitParamsToCone_( Eigen::Vector<T, Eigen::Dynamic>& fittedParams,
Cone3<T>& cone )
472 cone.apex().x = fittedParams[0];
473 cone.apex().y = fittedParams[1];
474 cone.apex().z = fittedParams[2];
476 cone.direction().x = fittedParams[3];
477 cone.direction().y = fittedParams[4];
478 cone.direction().z = fittedParams[5];
482 void coneToFitParams_(
Cone3<T>& cone, Eigen::Vector<T, Eigen::Dynamic>& fittedParams )
485 fittedParams[0] = cone.apex().x;
486 fittedParams[1] = cone.apex().y;
487 fittedParams[2] = cone.apex().z;
490 T coneCosAngle = std::cos( cone.angle );
491 fittedParams[3] = cone.direction().x / coneCosAngle;
492 fittedParams[4] = cone.direction().y / coneCosAngle;
493 fittedParams[5] = cone.direction().z / coneCosAngle;