void NearestNeighboursON2::getNearestNeighbours(
const Scan &queryScan,
unsigned int queryToReferenceMapping[][2],
double nearestDistances[]) {
const Scan &refScan = *(this->refScan);
const unsigned int queryScanSize = queryScan.size(), refScanSize = refScan.size();
for(unsigned int i = 0; i < queryScanSize; i++) {
TopValues<2> tv;
for(unsigned int j = 0; j < refScanSize; j++) {
double dist =
square(refScan[j].getX() - queryScan[i].getX()) +
square(refScan[j].getY() - queryScan[i].getY());
tv.add(dist, j);
}
queryToReferenceMapping[i][0] = tv.index(0);
queryToReferenceMapping[i][1] = tv.index(1);
nearestDistances[i] = tv.value(0);
}
}
void ICP::scanMatch(
const Scan &refScan,
const Scan &queryScan,
Eigen::Vector3d &rototranslation,
Eigen::Matrix3d &covariance,
const Eigen::Vector3d &rototranslationGuess) {
Scan rotatedScan;
Rototranslation2D RTcompose(rototranslationGuess);
unsigned int queryScanSize = queryScan.size();
unsigned int queryToReferenceMapping[queryScanSize][2];
double nearestDistances[queryScanSize];
const int maxIterations = config::icp_max_iter;
const double eps = config::icp_trim_ratio;
const double convError = config::icp_conv_error;
nearestNeighbours->initialize(refScan);
//std::cout << "ListAnimate[{";
for (int iteration = 0; iteration < maxIterations; iteration++) {
Rototranslation2D RT;
double distanceToTrim;
int trimSize;
Eigen::Vector3d q;
/* 1 - Rototranslate the second scan using the current RT estimate */
rotatedScan = RTcompose * queryScan;
/* 2 - Find for each (xxTi,yyTi) the corresponding segment */
nearestNeighbours->getNearestNeighbours(
rotatedScan, queryToReferenceMapping, nearestDistances);
/* 3 - Use a trimming procedure (The Trimmed Iterative Closest Point Algorithm) */
/* 3.1 - Order the distance in ascending order and select only eps*N pairs */
std::vector<double> distSorted(nearestDistances, nearestDistances + queryScanSize);
std::sort (distSorted.begin(), distSorted.end());
trimSize = (unsigned int) std::floor(eps * queryScanSize);
distanceToTrim = distSorted[trimSize - 1];
//std::cout << "Show[{Graphics[{Green";
/* 3.2 - Assign the right pairs in order to do the correct minimization */
unsigned int queryIndices[trimSize];
for (unsigned int j = 0, k = 0; k < queryScanSize; k++) {
if (nearestDistances[k] <= distanceToTrim) {
// TODO: WTF?
// Remove the bias given by matching always the first or last point
// if ((distNearest[k] < distanceToTrim) && (idxRef[k][0] != 0)
// && (idxRef[k][0] != static_cast<unsigned int>(sizeR-1))) {
queryIndices[j++] = k;
//std::cout << ",Line[{" << rotatedScan[k] << "," << refScan[queryToReferenceMapping[k][0]] << "}]";
}
}
//std::cout << "}]";
//std::cout << ",\nListPlot[{" << refScan << "," << rotatedScan <<
// "},AspectRatio->1,PlotStyle->{Red,Blue}]},PlotRange->{{-4,4},{-2,3}}]," << std::endl;
/* 4 - Optimize, find the optimal transformation */
q = this->getBestRototranslationVector(
refScan, rotatedScan, queryToReferenceMapping, queryIndices, trimSize);
/* 5 - Use compound to get the new RTCompose */
RT = Rototranslation2D(q).inverse();
RTcompose = RT * RTcompose;
//std::cout << "max(" << q[0] << "," << q[1] << "," << q[2] << ") < " << convError
// << "? " << (std::max(std::max(q[0], q[1]), q[2]) < convError) << std::endl;
/* 6 - Termination condition */
if(std::max(std::max(q[0], q[1]), q[2]) < convError) {
break;
}
}
//std::cout << "ListPlot[{" << refScan << "," << RTcompose * queryScan <<
// "},AspectRatio->1,PlotStyle->{Red,Blue},PlotRange->{{-4,4},{-2,3}}]}]" << std::endl;
nearestNeighbours->deinitialize();
rototranslation = RTcompose.getVectorForm();
covariance = this->computeCovariance(
refScan, queryScan, RTcompose, queryToReferenceMapping);
}
Eigen::Matrix3d PointToLineICP::computeCensiCovariance(
const Scan &refScan,
const Scan &queryScan,
const Rototranslation2D &rototranslation,
const unsigned int queryToReferenceMapping[][2]) {
// This routine assumes that only the range of the laser are subject to
// error and not the angle
const unsigned int refScanSize = refScan.size();
const unsigned int queryScanSize = queryScan.size();
const Eigen::Vector3d v = rototranslation.getVectorForm();
const double tx = v[0], ty = v[1], a = v[2];
// Hessian of point to line cost function wrt rototranslation
Eigen::Matrix3d H_xx = Eigen::Matrix3d::Zero();
// Hessian of point to line cost function wrt rototranslation and the norm
// (polar form) of the points from both scans
Eigen::MatrixXd H_xp = Eigen::MatrixXd::Zero(3, refScanSize + queryScanSize);
for(unsigned int i = 0; i < queryScanSize; i++) {
unsigned int j0 = queryToReferenceMapping[i][0];
unsigned int j1 = queryToReferenceMapping[i][1];
// Load the three points, two from the reference scan, one from the query
const Point &ref0 = refScan [j0];
const Point &ref1 = refScan [j1];
const Point &query = queryScan[i];
j0 += queryScanSize;
j1 += queryScanSize;
// Put the points in polar form
const double rho0 = ref0.getRho(), rho1 = ref1.getRho(), rhoq = query.getRho();
const double ct0 = ref0.getX() / rho0, st0 = ref0.getY() / rho0;
const double ct1 = ref1.getX() / rho1, st1 = ref1.getY() / rho1;
const double ctq = query.getX() / rhoq, stq = query.getY() / rhoq;
// Automatically optimised code for the computation of the Hessian wrt rototranslation
const double tmp_xx_0 = rho0*st0;
const double tmp_xx_1 = -(rho1*st1);
const double tmp_xx_2 = tmp_xx_0 + tmp_xx_1;
const double tmp_xx_3 = (tmp_xx_2*tmp_xx_2);
const double tmp_xx_4 = (rho0*rho0);
const double tmp_xx_5 = 1/((rho1*rho1) - 2*rho0*rho1*(ct0*ct1 + st0*st1) + tmp_xx_4);
const double tmp_xx_6 = ct0*rho0 - ct1*rho1;
const double tmp_xx_7 = -2*tmp_xx_2*tmp_xx_5*tmp_xx_6;
const double tmp_xx_8 = std::cos(a);
const double tmp_xx_9 = std::sin(a);
const double tmp_xx_10 = (ct0*ctq*rho0 + rho0*st0*stq - rho1*(ct1*ctq + st1*stq))*
tmp_xx_8 + (ctq*rho0*st0 - ctq*rho1*st1 - ct0*rho0*stq + ct1*rho1*stq)*tmp_xx_9;
const double tmp_xx_11 = -2*rhoq*tmp_xx_10*tmp_xx_2*tmp_xx_5;
const double tmp_xx_12 = 2*rhoq*tmp_xx_10*tmp_xx_5*tmp_xx_6;
const double tmp_xx_13 = stq*tmp_xx_8 + ctq*tmp_xx_9;
const double tmp_xx_14 = ctq*tmp_xx_8 - stq*tmp_xx_9;
const double tmp_xx_15 = -(rho0*st0);
// Component of Hessian wrt rototranslation
H_xx(0,0) += 2*tmp_xx_3*tmp_xx_5;
H_xx(0,1) += tmp_xx_7;
H_xx(0,2) += tmp_xx_11;
H_xx(1,0) += tmp_xx_7;
H_xx(1,1) += 2*tmp_xx_5*(tmp_xx_6*tmp_xx_6);
H_xx(1,2) += tmp_xx_12;
H_xx(2,0) += tmp_xx_11;
H_xx(2,1) += tmp_xx_12;
H_xx(2,2) += 2*tmp_xx_5*(std::pow(rhoq*tmp_xx_13*tmp_xx_2 + rhoq*tmp_xx_14*tmp_xx_6,2) +
(rhoq*tmp_xx_14*tmp_xx_2 - rhoq*tmp_xx_13*tmp_xx_6)* ((rho1*st1 + tmp_xx_15)*
(-(ct0*rho0) + ctq*rhoq*tmp_xx_8 - rhoq*stq*tmp_xx_9 + tx) + tmp_xx_6*
(tmp_xx_15 + rhoq*stq*tmp_xx_8 + ctq*rhoq*tmp_xx_9 + ty)));
// Automatically optimised code for the computation of the Hessian wrt rototranslation and
// points
const double tmp_xp_0 = -(rho0*st0);
const double tmp_xp_1 = rho1*st1;
const double tmp_xp_2 = tmp_xp_0 + tmp_xp_1;
const double tmp_xp_3 = (rho0*rho0);
const double tmp_xp_4 = (rho1*rho1);
const double tmp_xp_5 = ct0*ct1;
const double tmp_xp_6 = st0*st1 + tmp_xp_5;
const double tmp_xp_7 = -2*rho0*rho1*tmp_xp_6;
const double tmp_xp_8 = tmp_xp_3 + tmp_xp_4 + tmp_xp_7;
const double tmp_xp_9 = 1/tmp_xp_8;
const double tmp_xp_10 = ct0*rho0;
const double tmp_xp_11 = -(ct1*rho1) + tmp_xp_10;
const double tmp_xp_12 = std::cos(a);
const double tmp_xp_13 = std::sin(a);
const double tmp_xp_14 = stq*tmp_xp_12 + ctq*tmp_xp_13;
const double tmp_xp_15 = ctq*tmp_xp_12 - stq*tmp_xp_13;
const double tmp_xp_16 = tmp_xp_11*tmp_xp_14 + tmp_xp_15*tmp_xp_2;
const double tmp_xp_17 = std::pow(tmp_xp_8,-2);
const double tmp_xp_18 = ct0*ctq + st0*stq;
const double tmp_xp_19 = ct1*rho1*st0 - ct0*rho1*st1 + rhoq*(-(ctq*st0) + ct0*stq)*
tmp_xp_12 + rhoq*tmp_xp_13*tmp_xp_18 - st0*tx + ct0*ty;
const double tmp_xp_20 = -rho0 + ct0*ct1*rho1 + rho1*st0*st1;
const double tmp_xp_21 = tmp_xp_0 + rhoq*stq*tmp_xp_12 + ctq*rhoq*tmp_xp_13 + ty;
const double tmp_xp_22 = -(ct0*rho0) + ctq*rhoq*tmp_xp_12 - rhoq*stq*tmp_xp_13 + tx;
const double tmp_xp_23 = tmp_xp_11*tmp_xp_21 + tmp_xp_2*tmp_xp_22;
const double tmp_xp_24 = -(ct1*tmp_xp_21) + st1*tmp_xp_22;
const double tmp_xp_25 = ct0*ct1*rho0 - rho1 + rho0*st0*st1;
const double tmp_xp_26 = ct1*ctq + st1*stq;
const double tmp_xp_27 = ct0*ctq*rho0 + rho0*st0*stq - rho1*tmp_xp_26;
const double tmp_xp_28 = rho0*st0;
const double tmp_xp_29 = -(rho1*st1);
const double tmp_xp_30 = rhoq*tmp_xp_11*tmp_xp_15 + rhoq*tmp_xp_14*(tmp_xp_28 + tmp_xp_29);
// Component of Hessian wrt rototranslation and query point norm
H_xp(0,i) += 2*tmp_xp_16*tmp_xp_2*tmp_xp_9;
H_xp(1,i) += 2*tmp_xp_11*tmp_xp_16*tmp_xp_9;
H_xp(2,i) += 2*((ctq*rho0*st0 - ctq*rho1*st1 - ct0*rho0*stq + ct1*rho1*stq)*tmp_xp_13 +
tmp_xp_12*tmp_xp_27)*tmp_xp_9*(2*rhoq* ((-(ctq*rho0*st0) + ctq*rho1*st1 +
ct0*rho0*stq - ct1*rho1*stq)* tmp_xp_12 + tmp_xp_13*tmp_xp_27) - rho0*st0*tx
+ rho1*st1*tx + ct1*rho1*(tmp_xp_28 - ty) + ct0*rho0*(tmp_xp_29 + ty));
// Component of Hessian wrt rototranslation and reference point 1 norm
H_xp(0,j0) += 2*tmp_xp_17*(2*tmp_xp_2*tmp_xp_20*tmp_xp_23 + tmp_xp_19*tmp_xp_2*tmp_xp_8 -
st0*tmp_xp_23*tmp_xp_8);
H_xp(1,j0) += 2*tmp_xp_17*(2*tmp_xp_11*tmp_xp_20*tmp_xp_23 + tmp_xp_11*tmp_xp_19*tmp_xp_8 +
ct0*tmp_xp_23*tmp_xp_8);
H_xp(2,j0) += 2*tmp_xp_17*(2*tmp_xp_20*tmp_xp_23*tmp_xp_30 + rhoq*((ctq*st0 - ct0*stq)*
tmp_xp_13 + tmp_xp_12*tmp_xp_18)*tmp_xp_23*tmp_xp_8 + tmp_xp_19*tmp_xp_30*tmp_xp_8);
// Component of Hessian wrt rototranslation and reference point 2 norm
H_xp(0,j1) += 2*tmp_xp_17*(2*tmp_xp_2*tmp_xp_23*tmp_xp_25 + st1*tmp_xp_23*tmp_xp_8 +
tmp_xp_2*tmp_xp_24*tmp_xp_8);
H_xp(1,j1) += 2*tmp_xp_17*(2*tmp_xp_11*tmp_xp_23*tmp_xp_25 - ct1*tmp_xp_23*tmp_xp_8 +
tmp_xp_11*tmp_xp_24*tmp_xp_8);
H_xp(2,j1) += 2*tmp_xp_17*(2*tmp_xp_23*tmp_xp_25*tmp_xp_30 - rhoq*tmp_xp_23*((ctq*st1 -
ct1*stq)*tmp_xp_13 + tmp_xp_12*tmp_xp_26)*tmp_xp_8 + tmp_xp_24*tmp_xp_30*tmp_xp_8);
}
// Jacobian of minimisation algorithm wrt the data (the norm of the scan points)
Eigen::MatrixXd J = H_xx.inverse() * H_xp;
// Covariance is J * R * J.transpose(), where R is the covariance of the input data; if we
// assume all point norms to be uncorrelated and with variance sigma2 it becomes:
return config::sigma2 * J * J.transpose();
}