216 lines
6.9 KiB
C++
216 lines
6.9 KiB
C++
// 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);
|
|
}
|
|
/*----------------------------------------------------------------*/
|