C++ Scan::size方法代码示例

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;
//.........这里部分代码省略.........