50 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
51 assert(eigenObservation.cols() == 4);
52 size_t nObservations = observations2D.
size();
57 Eigen::Matrix<double, Eigen::Dynamic, 2> points = eigenObservation.leftCols<2>();
59 Eigen::Matrix<double, 1, 2> pointMean;
62 pointMean << 0.0, 0.0;
65 pointMean = points.colwise().mean();
67 points = points.rowwise() - pointMean;
69 Eigen::Matrix<double, 2, 2> covarianceMatrix = points.transpose() * points;
71 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 2, 2> > eigensolver(covarianceMatrix);
73 if (eigensolver.info() != Eigen::Success)
74 B2WARNING(
"SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
79 Eigen::Matrix<double, 2, 1> normalToLine = eigensolver.eigenvectors().col(0);
81 double offset = -pointMean * normalToLine;
90 Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
92 projectedPoints.col(0) = eigenObservation.col(0);
93 projectedPoints.col(1) = eigenObservation.col(1);
94 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm();
97 Eigen::Matrix<double, 1, 3> pointMean;
99 pointMean << 0.0, 0.0, 0.0;
102 pointMean = projectedPoints.colwise().mean();
104 projectedPoints = projectedPoints.rowwise() - pointMean;
107 Eigen::Matrix<double, 3, 3> covarianceMatrix = projectedPoints.transpose() * projectedPoints;
109 Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, 3, 3> > eigensolver(covarianceMatrix);
111 if (eigensolver.info() != Eigen::Success)
112 B2WARNING(
"Eigen::SelfAdjointEigenSolver could not compute the eigen values of the observation matrix");
117 Eigen::Matrix<double, 3, 1> normalToPlane = eigensolver.eigenvectors().col(0);
119 double offset = -pointMean * normalToPlane;
131 size_t voteForChangeSign = 0;
132 for (
size_t iPoint = 0; iPoint < nObservations; ++iPoint) {
133 double pointInSameDirection = eigenObservation(iPoint, 0) * directionAtCenter.
x() +
134 eigenObservation(iPoint, 1) * directionAtCenter.
y();
135 if (pointInSameDirection < 0) ++voteForChangeSign;
138 if (voteForChangeSign > nObservations / 2.0) trajectory2D.
reverse();
146 EigenObservationMatrix eigenObservation = getEigenObservationMatrix(&observations2D);
147 assert(eigenObservation.cols() == 4);
148 size_t nObservations = observations2D.
size();
158 Eigen::Matrix< double, Eigen::Dynamic, 1 > distances = eigenObservation.col(2);
165 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 2);
169 projectedPoints.col(0) = eigenObservation.col(0);
170 projectedPoints.col(1) = eigenObservation.col(1);
173 Eigen::Matrix<double, 2, 1> parameters =
174 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
181 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
185 projectedPoints.col(0) = eigenObservation.col(0);
186 projectedPoints.col(1) = eigenObservation.col(1);
187 projectedPoints.col(2) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
189 Eigen::Matrix<double, 3, 1> parameters =
190 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
197 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 3);
200 projectedPoints.col(0) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
201 projectedPoints.col(1) = eigenObservation.col(0);
202 projectedPoints.col(2) = eigenObservation.col(1);
205 Eigen::Matrix<double, 3, 1> parameters =
206 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);
215 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > projectedPoints(nObservations, 4);
218 projectedPoints.col(0) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
219 projectedPoints.col(1) = eigenObservation.col(0);
220 projectedPoints.col(2) = eigenObservation.col(1);
221 projectedPoints.col(3) = eigenObservation.leftCols<2>().rowwise().squaredNorm() - distances.rowwise().squaredNorm();
225 Eigen::Matrix<double, 4, 1> parameters =
226 projectedPoints.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(distances);