| 1 |
#ifndef CYLINDER_HEADER |
| 2 |
#define CYLINDER_HEADER |
| 3 |
#include "basic.h" |
| 4 |
#include <stdexcept> |
| 5 |
#include <utility> |
| 6 |
#include <MiscLib/Vector.h> |
| 7 |
#include "PointCloud.h" |
| 8 |
#include <ostream> |
| 9 |
#include <istream> |
| 10 |
#include <GfxTL/HyperplaneCoordinateSystem.h> |
| 11 |
#include <stdio.h> |
| 12 |
#include <MiscLib/NoShrinkVector.h> |
| 13 |
#include "LevMarLSWeight.h" |
| 14 |
#include "LevMarFitting.h" |
| 15 |
|
| 16 |
#ifndef DLL_LINKAGE |
| 17 |
#define DLL_LINKAGE |
| 18 |
#endif |
| 19 |
|
| 20 |
class DLL_LINKAGE Cylinder |
| 21 |
{ |
| 22 |
public: |
| 23 |
struct ParallelNormalsError |
| 24 |
: public std::runtime_error |
| 25 |
{ |
| 26 |
ParallelNormalsError(); |
| 27 |
}; |
| 28 |
enum { RequiredSamples = 2 }; |
| 29 |
Cylinder(); |
| 30 |
Cylinder(const Vec3f &axisDir, const Vec3f &axisPos, float radius); |
| 31 |
Cylinder(const Vec3f &pointA, const Vec3f &pointB, |
| 32 |
const Vec3f &normalA, const Vec3f &normalB); |
| 33 |
bool Init(const MiscLib::Vector< Vec3f > &samples); |
| 34 |
bool InitAverage(const MiscLib::Vector< Vec3f > &samples); |
| 35 |
bool Init(const Vec3f &axisDir, const Vec3f &axisPos, float radius); |
| 36 |
bool Init(const Vec3f &pointA, const Vec3f &pointB, |
| 37 |
const Vec3f &normalA, const Vec3f &normalB); |
| 38 |
bool Init(bool binary, std::istream *i); |
| 39 |
void Init(FILE *i); |
| 40 |
void Init(float* array); |
| 41 |
inline float Distance(const Vec3f &p) const; |
| 42 |
inline void Normal(const Vec3f &p, Vec3f *normal) const; |
| 43 |
inline float DistanceAndNormal(const Vec3f &p, Vec3f *normal) const; |
| 44 |
inline float SignedDistance(const Vec3f &p) const; |
| 45 |
void Project(const Vec3f &p, Vec3f *pp) const; |
| 46 |
// parameters are (height, angle) |
| 47 |
void Parameters(const Vec3f &p, |
| 48 |
std::pair< float, float > *param) const; |
| 49 |
float Radius() const; |
| 50 |
float &Radius(); |
| 51 |
const Vec3f &AxisDirection() const; |
| 52 |
Vec3f &AxisDirection(); |
| 53 |
const Vec3f &AxisPosition() const; |
| 54 |
Vec3f &AxisPosition(); |
| 55 |
const Vec3f AngularDirection() const; |
| 56 |
void RotateAngularDirection(float radians); |
| 57 |
bool LeastSquaresFit(const PointCloud &pc, |
| 58 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 59 |
MiscLib::Vector< size_t >::const_iterator end); |
| 60 |
template< class IteratorT > |
| 61 |
bool LeastSquaresFit(IteratorT begin, IteratorT end); |
| 62 |
bool Fit(const PointCloud &pc, |
| 63 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 64 |
MiscLib::Vector< size_t >::const_iterator end) |
| 65 |
{ return LeastSquaresFit(pc, begin, end); } |
| 66 |
static bool Interpolate(const MiscLib::Vector< Cylinder > &cylinders, |
| 67 |
const MiscLib::Vector< float > &weights, Cylinder *ic); |
| 68 |
void Serialize(bool binary, std::ostream *o) const; |
| 69 |
static size_t SerializedSize(); |
| 70 |
void Serialize(FILE *o) const; |
| 71 |
void Serialize(float* array) const; |
| 72 |
static size_t SerializedFloatSize(); |
| 73 |
|
| 74 |
void Transform(float scale, const Vec3f &translate); |
| 75 |
void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot, |
| 76 |
const GfxTL::Vector3Df &trans); |
| 77 |
inline unsigned int Intersect(const Vec3f &p, const Vec3f &r, |
| 78 |
float *first, float *second) const; |
| 79 |
|
| 80 |
private: |
| 81 |
template< class WeightT > |
| 82 |
class LevMarCylinder |
| 83 |
: public WeightT |
| 84 |
{ |
| 85 |
public: |
| 86 |
enum { NumParams = 7 }; |
| 87 |
typedef float ScalarType; |
| 88 |
|
| 89 |
template< class IteratorT > |
| 90 |
ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end, |
| 91 |
ScalarType *values, ScalarType *temp) const |
| 92 |
{ |
| 93 |
ScalarType chi = 0; |
| 94 |
size_t size = end - begin; |
| 95 |
#ifdef DOPARALLEL |
| 96 |
#pragma omp parallel for schedule(static) reduction(+:chi) |
| 97 |
#endif |
| 98 |
for(int idx = 0; idx < size; ++idx) |
| 99 |
{ |
| 100 |
Vec3f s; |
| 101 |
for(unsigned int j = 0; j < 3; ++j) |
| 102 |
s[j] = begin[idx][j] - params[j]; |
| 103 |
ScalarType u = params[5] * s[1] - params[4] * s[2]; |
| 104 |
u *= u; |
| 105 |
ScalarType v = params[3] * s[2] - params[5] * s[0]; |
| 106 |
u += v * v; |
| 107 |
v = params[4] * s[0] - params[3] * s[1]; |
| 108 |
u += v * v; |
| 109 |
temp[idx] = std::sqrt(u); |
| 110 |
chi += (values[idx] = WeightT::Weigh(temp[idx] - params[6])) |
| 111 |
* values[idx]; |
| 112 |
} |
| 113 |
return chi; |
| 114 |
} |
| 115 |
|
| 116 |
template< class IteratorT > |
| 117 |
void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end, |
| 118 |
const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const |
| 119 |
{ |
| 120 |
size_t size = end - begin; |
| 121 |
#ifdef DOPARALLEL |
| 122 |
#pragma omp parallel for schedule(static) |
| 123 |
#endif |
| 124 |
for(int idx = 0; idx < size; ++idx) |
| 125 |
{ |
| 126 |
Vec3f s; |
| 127 |
for(unsigned int j = 0; j < 3; ++j) |
| 128 |
s[j] = begin[idx][j] - params[j]; |
| 129 |
ScalarType g = s[0] * begin[idx][0] + s[1] * begin[idx][1] |
| 130 |
+ s[2] * begin[idx][2]; |
| 131 |
if(temp[idx] < 1.0e-6) |
| 132 |
{ |
| 133 |
matrix[idx * NumParams + 0] = std::sqrt(1 - params[3] * params[3]); |
| 134 |
matrix[idx * NumParams + 1] = std::sqrt(1 - params[4] * params[4]); |
| 135 |
matrix[idx * NumParams + 2] = std::sqrt(1 - params[5] * params[5]); |
| 136 |
} |
| 137 |
else |
| 138 |
{ |
| 139 |
matrix[idx * NumParams + 0] = (params[3] * g - s[0]) / temp[idx]; |
| 140 |
matrix[idx * NumParams + 1] = (params[4] * g - s[1]) / temp[idx]; |
| 141 |
matrix[idx * NumParams + 2] = (params[5] * g - s[2]) / temp[idx]; |
| 142 |
} |
| 143 |
matrix[idx * NumParams + 3] = g * matrix[idx * NumParams + 0]; |
| 144 |
matrix[idx * NumParams + 4] = g * matrix[idx * NumParams + 1]; |
| 145 |
matrix[idx * NumParams + 5] = g * matrix[idx * NumParams + 2]; |
| 146 |
matrix[idx * NumParams + 6] = -1; |
| 147 |
WeightT::template DerivWeigh< NumParams >(temp[idx] - params[6], |
| 148 |
matrix + idx * NumParams); |
| 149 |
} |
| 150 |
} |
| 151 |
|
| 152 |
void Normalize(ScalarType *params) const |
| 153 |
{ |
| 154 |
ScalarType l = std::sqrt(params[3] * params[3] + params[4] * params[4] |
| 155 |
+ params[5] * params[5]); |
| 156 |
for(unsigned int i = 3; i < 6; ++i) |
| 157 |
params[i] /= l; |
| 158 |
// find point on axis closest to origin |
| 159 |
float lambda = -(params[0] * params[3] + params[1] * params[4] + |
| 160 |
params[2] * params[5]); |
| 161 |
for(unsigned int i = 0; i < 3; ++i) |
| 162 |
params[i] = params[i] + lambda * params[i + 3]; |
| 163 |
} |
| 164 |
}; |
| 165 |
|
| 166 |
private: |
| 167 |
Vec3f m_axisDir; |
| 168 |
Vec3f m_axisPos; |
| 169 |
float m_radius; |
| 170 |
GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs; |
| 171 |
float m_angularRotatedRadians; |
| 172 |
}; |
| 173 |
|
| 174 |
inline float Cylinder::Distance(const Vec3f &p) const |
| 175 |
{ |
| 176 |
Vec3f diff = p - m_axisPos; |
| 177 |
float lambda = m_axisDir.dot(diff); |
| 178 |
float axisDist = (diff - lambda * m_axisDir).length(); |
| 179 |
return fabs(axisDist - m_radius); |
| 180 |
} |
| 181 |
|
| 182 |
inline void Cylinder::Normal(const Vec3f &p, Vec3f *normal) const |
| 183 |
{ |
| 184 |
Vec3f diff = p - m_axisPos; |
| 185 |
float lambda = m_axisDir.dot(diff); |
| 186 |
*normal = diff - lambda * m_axisDir; |
| 187 |
normal->normalize(); |
| 188 |
} |
| 189 |
|
| 190 |
inline float Cylinder::DistanceAndNormal(const Vec3f &p, Vec3f *normal) const |
| 191 |
{ |
| 192 |
Vec3f diff = p - m_axisPos; |
| 193 |
float lambda = m_axisDir.dot(diff); |
| 194 |
*normal = diff - lambda * m_axisDir; |
| 195 |
float axisDist = normal->length(); |
| 196 |
if(axisDist > 0) |
| 197 |
*normal /= axisDist; |
| 198 |
return fabs(axisDist - m_radius); |
| 199 |
} |
| 200 |
|
| 201 |
inline float Cylinder::SignedDistance(const Vec3f &p) const |
| 202 |
{ |
| 203 |
Vec3f diff = p - m_axisPos; |
| 204 |
float lambda = m_axisDir.dot(diff); |
| 205 |
float axisDist = (diff - lambda * m_axisDir).length(); |
| 206 |
return axisDist - m_radius; |
| 207 |
} |
| 208 |
|
| 209 |
template< class IteratorT > |
| 210 |
bool Cylinder::LeastSquaresFit(IteratorT begin, IteratorT end) |
| 211 |
{ |
| 212 |
float param[7]; |
| 213 |
for(size_t i = 0; i < 3; ++i) |
| 214 |
param[i] = m_axisPos[i]; |
| 215 |
for(size_t i = 0; i < 3; ++i) |
| 216 |
param[i + 3] = m_axisDir[i]; |
| 217 |
param[6] = m_radius; |
| 218 |
LevMarCylinder< LevMarLSWeight > levMarCylinder; |
| 219 |
if(!LevMar(begin, end, levMarCylinder, param)) |
| 220 |
return false; |
| 221 |
for(size_t i = 0; i < 3; ++i) |
| 222 |
m_axisPos[i] = param[i]; |
| 223 |
for(size_t i = 0; i < 3; ++i) |
| 224 |
m_axisDir[i] = param[i + 3]; |
| 225 |
m_radius = param[6]; |
| 226 |
m_hcs.FromNormal(m_axisDir); |
| 227 |
m_angularRotatedRadians = 0; |
| 228 |
return true; |
| 229 |
} |
| 230 |
|
| 231 |
inline unsigned int Cylinder::Intersect(const Vec3f &p, const Vec3f &r, |
| 232 |
float *first, float *second) const |
| 233 |
{ |
| 234 |
using namespace std; |
| 235 |
// Create a coordinate system for the cylinder. In this system, the |
| 236 |
// cylinder segment center C is the origin and the cylinder axis direction |
| 237 |
// W is the z-axis. U and V are the other coordinate axis directions. |
| 238 |
// If P = x*U+y*V+z*W, the cylinder is x^2 + y^2 = r^2, where r is the |
| 239 |
// cylinder radius. The end caps are |z| = h/2, where h is the cylinder |
| 240 |
// height. |
| 241 |
float fRSqr = m_radius * m_radius; |
| 242 |
|
| 243 |
// convert incoming line origin to cylinder coordinates |
| 244 |
Vec3f kDiff = p - m_axisPos; |
| 245 |
Vec3f kP(kDiff.dot(m_hcs[0]), kDiff.dot(m_hcs[1]), m_axisDir.dot(kDiff)); |
| 246 |
|
| 247 |
// Get the z-value, in cylinder coordinates, of the incoming line's |
| 248 |
// unit-length direction. |
| 249 |
float fDz = m_axisDir.dot(r); |
| 250 |
|
| 251 |
if(fabs(fDz) >= 1.f - 1e-7f) |
| 252 |
// The line is parallel to the cylinder axis. |
| 253 |
return 0; |
| 254 |
|
| 255 |
// convert incoming line unit-length direction to cylinder coordinates |
| 256 |
Vec3f kD(r.dot(m_hcs[0]), r.dot(m_hcs[1]), r.dot(m_axisDir)); |
| 257 |
|
| 258 |
float fA0, fA1, fA2, fDiscr, fRoot, fInv; |
| 259 |
|
| 260 |
// Test intersection of line P+t*D with infinite cylinder |
| 261 |
// x^2+y^2 = r^2. This reduces to computing the roots of a |
| 262 |
// quadratic equation. If P = (px,py,pz) and D = (dx,dy,dz), |
| 263 |
// then the quadratic equation is |
| 264 |
// (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0 |
| 265 |
fA0 = kP[0]*kP[0] + kP[1]*kP[1] - fRSqr; |
| 266 |
fA1 = kP[0]*kD[0] + kP[1]*kD[1]; |
| 267 |
fA2 = kD[0]*kD[0] + kD[1]*kD[1]; |
| 268 |
fDiscr = fA1*fA1 - fA0*fA2; |
| 269 |
if (fDiscr < 0) |
| 270 |
// line does not intersect cylinder |
| 271 |
return 0; |
| 272 |
else if (fDiscr > 1e-7f) |
| 273 |
{ |
| 274 |
// line intersects cylinder in two places |
| 275 |
fRoot = sqrt(fDiscr); |
| 276 |
fInv = (1.f)/fA2; |
| 277 |
*first = (-fA1 - fRoot)*fInv; |
| 278 |
*second = (-fA1 + fRoot)*fInv; |
| 279 |
return 2; |
| 280 |
} |
| 281 |
// line is tangent to the cylinder |
| 282 |
*first = -fA1/fA2; |
| 283 |
return 1; |
| 284 |
} |
| 285 |
|
| 286 |
#endif |