提交scripts
This commit is contained in:
hesuicong committed
1 parent
82caf72036
commit
07dabbd186
224 files changed
+130146
No files matched your search
@@ -0,0 +1,215 @@
|
||||
// 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);
|
||||
}
|
||||
/*----------------------------------------------------------------*/
|
||||
Reference in new issue
Block a user