You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
215 lines
6.9 KiB
215 lines
6.9 KiB
// D E F I N E S /////////////////////////////////////////////////// |
|
|
|
|
|
// S T R U C T S /////////////////////////////////////////////////// |
|
|
|
|
|
// C L A S S ////////////////////////////////////////////////////// |
|
|
|
template <typename TYPE, int DIMS> |
|
inline TLine<TYPE,DIMS>::TLine(const POINT& _pt1, const POINT& _pt2) |
|
: |
|
pt1(_pt1), pt2(_pt2) |
|
{ |
|
ASSERT(!ISZERO((_pt1-_pt2).norm())); |
|
} // constructor |
|
template <typename TYPE, int DIMS> |
|
template <typename CTYPE> |
|
inline TLine<TYPE,DIMS>::TLine(const TLine<CTYPE,DIMS>& rhs) |
|
: |
|
pt1(rhs.pt1.template cast<TYPE>()), pt2(rhs.pt2.template cast<TYPE>()) |
|
{ |
|
} // copy constructor |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// set attributes |
|
template <typename TYPE, int DIMS> |
|
inline void TLine<TYPE,DIMS>::Set(const POINT& _pt1, const POINT& _pt2) |
|
{ |
|
ASSERT(!ISZERO((_pt1-_pt2).norm())); |
|
pt1 = _pt1; |
|
pt2 = _pt2; |
|
} |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// least squares refinement of the line to the given 3D point set |
|
// (return the number of iterations) |
|
template <typename TYPE, int DIMS> |
|
template <typename RobustNormFunctor> |
|
int TLine<TYPE,DIMS>::Optimize(const POINT* points, size_t size, const RobustNormFunctor& robust, int maxIters) |
|
{ |
|
ASSERT(DIMS == 3); |
|
ASSERT(size >= numParams); |
|
struct OptimizationFunctor { |
|
const POINT* points; |
|
size_t size; |
|
double scale; |
|
const RobustNormFunctor& robust; |
|
// construct with the data points |
|
OptimizationFunctor(const POINT* _points, size_t _size, const RobustNormFunctor& _robust) |
|
: points(_points), size(_size), robust(_robust) { ASSERT(size < std::numeric_limits<int>::max()); } |
|
static void Residuals(const double* x, int nPoints, const void* pData, double* fvec, double* fjac, int* /*info*/) { |
|
const OptimizationFunctor& data = *reinterpret_cast<const OptimizationFunctor*>(pData); |
|
ASSERT((size_t)nPoints == data.size && fvec != NULL && fjac == NULL); |
|
TLine<double,DIMS> line; |
|
for (int j=0; j<DIMS; ++j) |
|
line.pt1(j) = x[j]; |
|
DirScale2Vector(x+DIMS, &data.scale, line.pt2.data()); |
|
line.pt2 += line.pt1; |
|
for (size_t i=0; i<data.size; ++i) |
|
fvec[i] = data.robust(line.Distance(data.points[i].template cast<double>())); |
|
} |
|
} functor(points, size, robust); |
|
double arrParams[numParams]; |
|
for (int j=0; j<DIMS; ++j) |
|
arrParams[j] = (double)pt1(j); |
|
POINT dir(pt2-pt1); |
|
Vector2DirScale(dir.data(), arrParams+DIMS, &functor.scale); |
|
lm_control_struct control = {1.e-6, 1.e-7, 1.e-8, 1.e-8, 100.0, maxIters}; // lm_control_float; |
|
lm_status_struct status; |
|
lmmin(numParams, arrParams, (int)size, &functor, OptimizationFunctor::Residuals, &control, &status); |
|
switch (status.info) { |
|
//case 4: |
|
case 5: |
|
case 6: |
|
case 7: |
|
case 8: |
|
case 9: |
|
case 10: |
|
case 11: |
|
case 12: |
|
DEBUG_ULTIMATE("error: refine line: %s", lm_infmsg[status.info]); |
|
return 0; |
|
} |
|
for (int j=0; j<DIMS; ++j) |
|
pt1(j) = (TYPE)arrParams[j]; |
|
DirScale2Vector(arrParams+DIMS, &functor.scale, dir.data()); |
|
pt2 = pt1+dir; |
|
return status.nfev; |
|
} |
|
template <typename TYPE, int DIMS> |
|
int TLine<TYPE,DIMS>::Optimize(const POINT* points, size_t size, int maxIters) |
|
{ |
|
const auto identity = [](double x) { return x; }; |
|
return Optimize(points, size, identity, maxIters); |
|
} // Optimize |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// get attributes |
|
template <typename TYPE, int DIMS> |
|
inline TYPE TLine<TYPE,DIMS>::GetLength() const |
|
{ |
|
return (pt2 - pt1).norm(); |
|
} |
|
template <typename TYPE, int DIMS> |
|
inline TYPE TLine<TYPE,DIMS>::GetLengthSq() const |
|
{ |
|
return (pt2 - pt1).squaredNorm(); |
|
} |
|
template <typename TYPE, int DIMS> |
|
inline typename TLine<TYPE,DIMS>::POINT TLine<TYPE,DIMS>::GetCenter() const |
|
{ |
|
return (pt2 + pt1) / TYPE(2); |
|
} |
|
template <typename TYPE, int DIMS> |
|
inline typename TLine<TYPE,DIMS>::VECTOR TLine<TYPE,DIMS>::GetDir() const |
|
{ |
|
return (pt2 - pt1); |
|
} |
|
template <typename TYPE, int DIMS> |
|
inline typename TLine<TYPE,DIMS>::VECTOR TLine<TYPE,DIMS>::GetNormDir() const |
|
{ |
|
return (pt2 - pt1).normalized(); |
|
} |
|
template <typename TYPE, int DIMS> |
|
inline typename TLine<TYPE,DIMS>::RAY TLine<TYPE,DIMS>::GetRay() const |
|
{ |
|
return RAY(pt1, GetNormDir()); |
|
} |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
template <typename TYPE, int DIMS> |
|
inline bool TLine<TYPE,DIMS>::IsSame(const TLine& line, TYPE th) const |
|
{ |
|
const TYPE thSq(SQUARE(th)); |
|
const VECTOR l(pt2-pt1); |
|
const TYPE invLenSq(INVERT(l.squaredNorm())); |
|
const VECTOR r1(pt1-line.pt1); |
|
const TYPE dSq1((l.cross(r1)).squaredNorm()*invLenSq); |
|
if (dSq1 > thSq) |
|
return false; |
|
const VECTOR r2(pt1-line.pt2); |
|
const TYPE dSq2((l.cross(r2)).squaredNorm()*invLenSq); |
|
return dSq2 <= thSq; |
|
} |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// test for intersection with aabb |
|
template <typename TYPE, int DIMS> |
|
bool TLine<TYPE,DIMS>::Intersects(const AABB &aabb) const |
|
{ |
|
return GetRay().Intersects(aabb); |
|
} // Intersects(AABB) |
|
template <typename TYPE, int DIMS> |
|
bool TLine<TYPE,DIMS>::Intersects(const AABB &aabb, TYPE& t) const |
|
{ |
|
return GetRay().Intersects(aabb, t); |
|
} // Intersects(AABB) |
|
/*----------------------------------------------------------------*/ |
|
|
|
// Computes the distance between the line and a point. |
|
template <typename TYPE, int DIMS> |
|
inline TYPE TLine<TYPE,DIMS>::DistanceSq(const POINT& pt) const |
|
{ |
|
const VECTOR l(pt2-pt1), r(pt1-pt); |
|
if (DIMS == 2) |
|
return TYPE(SQUARE(l[0]*r[1]-r[0]*l[1])/(l[0]*l[0]+l[1]*l[1])); |
|
ASSERT(DIMS == 3); |
|
return TYPE((l.cross(r)).squaredNorm()/l.squaredNorm()); |
|
} // DistanceSq(POINT) |
|
template <typename TYPE, int DIMS> |
|
inline TYPE TLine<TYPE,DIMS>::Distance(const POINT& pt) const |
|
{ |
|
return SQRT(DistanceSq(pt)); |
|
} // Distance(POINT) |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// Computes the position on the line segment of the point projection. |
|
// Returns 0 if it coincides with the first point, and 1 if it coincides with the second point. |
|
template <typename TYPE, int DIMS> |
|
inline TYPE TLine<TYPE,DIMS>::Classify(const POINT& p) const |
|
{ |
|
const VECTOR vL(pt2 - pt1); |
|
ASSERT(!ISZERO(vL.squaredNorm())); |
|
const VECTOR vP(p - pt1); |
|
return vL.dot(vP) / vL.squaredNorm(); |
|
} // Classify(POINT) |
|
// Calculate point's projection on this line (closest point to this line). |
|
template <typename TYPE, int DIMS> |
|
inline typename TLine<TYPE,DIMS>::POINT TLine<TYPE,DIMS>::ProjectPoint(const POINT& p) const |
|
{ |
|
const VECTOR vL(pt2 - pt1); |
|
ASSERT(!ISZERO(vL.squaredNorm())); |
|
const VECTOR vP(p - pt1); |
|
return pt1 + vL * (vL.dot(vP) / vL.squaredNorm()); |
|
} // ProjectPoint |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
template <typename TYPE, typename TYPEW> |
|
template <typename TYPEE> |
|
TPoint3<TYPEE> FitLineOnline<TYPE,TYPEW>::GetLine(TLine<TYPEE,3>& line) const |
|
{ |
|
TPoint3<TYPEW> avg, dir; |
|
const TPoint3<TYPEW> quality(this->GetModel(avg, dir)); |
|
const TPoint3<TYPEW> pt2(avg+dir); |
|
line.Set(TPoint3<TYPEE>(avg), TPoint3<TYPEE>(pt2)); |
|
return TPoint3<TYPEE>(quality); |
|
} |
|
/*----------------------------------------------------------------*/
|
|
|