21 const auto& inner = innerClusters[tracklet.firstClusterIndex];
24 const float origin[3] = {inner.
xCoordinate, inner.yCoordinate, inner.zCoordinate};
25 const float direction[3] = {outer.xCoordinate - inner.xCoordinate,
26 outer.yCoordinate - inner.yCoordinate,
27 outer.zCoordinate - inner.zCoordinate};
40 const float n[3] = {
a[1] *
b[2] -
a[2] *
b[1],
41 a[2] *
b[0] -
a[0] *
b[2],
42 a[0] *
b[1] -
a[1] *
b[0]};
43 const float norm2 =
n[0] *
n[0] +
n[1] *
n[1] +
n[2] *
n[2];
46 for (
int i = 0;
i < 3; ++
i) {
53 for (
int i = 0;
i < 3; ++
i) {
57 for (
int i = 0;
i < 3; ++
i) {
58 const float residual = d[
i] - proj *
a[
i];
59 res2 += residual * residual;
64 const float numerator = d[0] *
n[0] + d[1] *
n[1] + d[2] *
n[2];
65 return (numerator * numerator) / norm2;
75 LOGP(info,
"\tLine: originPoint = ({}, {}, {}), cosinesDirector = ({}, {}, {}) ts={}+-{}",
78 mTime.getTimeStamp(),
mTime.getTimeStampError());
83 if (lineIndices.size() < 2) {
86 for (
const auto idx : lineIndices) {
89 computeClusterCentroid();
93 for (
const auto idx : lineIndices) {
94 addResidual(lines[idx]);
100 for (
int i = 0;
i < 6; ++
i) {
101 if (mRMS2[
i] != rhs.mRMS2[
i]) {
105 for (
int i = 0;
i < 3; ++
i) {
106 if (mVertex[
i] != rhs.mVertex[
i]) {
110 return mNContributors == rhs.mNContributors && mAvgDistance2 == rhs.mAvgDistance2;
Least-squares vertex fit over a set of lines (the normal equations AX = -B).
bool operator==(const ClusterLines &rhs) const noexcept
ClusterLines(gsl::span< const int > lineIndices, gsl::span< const Line > lines)
Fit over lines[lineIndices[i]]; lineIndices must be sorted and unique.
GLboolean GLboolean GLboolean b
GLboolean GLboolean GLboolean GLboolean a
GLenum GLint GLint * precision
const float const float const float const float std::array< float, 3 > & origin
static float getDistance2FromPoint(const Line &line, const std::array< float, 3 > &point)
static float getDCA2(const Line &, const Line &, const float precision=constants::Tolerance)
static float getDCA(const Line &, const Line &, const float precision=constants::Tolerance)
Line(const Tracklet &, const Cluster *, const Cluster *)
static float getDistanceFromPoint(const Line &line, const std::array< float, 3 > &point)