C++ Ref::cols方法代码示例

// apply the distmesh algorithm
std::tuple<Eigen::ArrayXXd, Eigen::ArrayXXi> distmesh::distmesh(
    Functional const& distanceFunction, double const initialPointDistance,
    Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
    Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
    // determine dimension of mesh
    unsigned const dimension = boundingBox.cols();

    // create initial distribution in bounding box
    Eigen::ArrayXXd points = utils::createInitialPoints(distanceFunction,
        initialPointDistance, elementSizeFunction, boundingBox, fixedPoints);

    // create initial triangulation
    Eigen::ArrayXXi triangulation = triangulation::delaunay(points);

    // create buffer to store old point locations to calculate
    // retriangulation and stop criterion
    Eigen::ArrayXXd retriangulationCriterionBuffer = Eigen::ArrayXXd::Constant(
        points.rows(), points.cols(), INFINITY);
    Eigen::ArrayXXd stopCriterionBuffer = Eigen::ArrayXXd::Zero(
        points.rows(), points.cols());

    // main distmesh loop
    Eigen::ArrayXXi edgeIndices;
    for (unsigned step = 0; step < constants::maxSteps; ++step) {
        // retriangulate if point movement is above threshold
        if ((points - retriangulationCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() >
            constants::retriangulationThreshold * initialPointDistance) {
            // update triangulation
            triangulation = triangulation::delaunay(points);

            // reject triangles with circumcenter outside of the region
            Eigen::ArrayXXd circumcenter = Eigen::ArrayXXd::Zero(triangulation.rows(), dimension);
            for (int point = 0; point < triangulation.cols(); ++point) {
                circumcenter += utils::selectIndexedArrayElements<double>(
                    points, triangulation.col(point)) / triangulation.cols();
            }
            triangulation = utils::selectMaskedArrayElements<int>(triangulation,
                distanceFunction(circumcenter) < -constants::geometryEvaluationThreshold * initialPointDistance);

            // find unique edge indices
            edgeIndices = utils::findUniqueEdges(triangulation);

            // store current points positions
            retriangulationCriterionBuffer = points;
        }

        // calculate edge vectors and their length
        auto const edgeVector = (utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) -
            utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1))).eval();
        auto const edgeLength = edgeVector.square().rowwise().sum().sqrt().eval();

        // evaluate elementSizeFunction at midpoints of edges
        auto const desiredElementSize = elementSizeFunction(0.5 *
            (utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) +
            utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1)))).eval();

        // calculate desired edge length
        auto const desiredEdgeLength = (desiredElementSize * (1.0 + 0.4 / std::pow(2.0, dimension - 1)) *
            std::pow((edgeLength.pow(dimension).sum() / desiredElementSize.pow(dimension).sum()),
                1.0 / dimension)).eval();

        // calculate force vector for each edge
        auto const forceVector = (edgeVector.colwise() *
            ((desiredEdgeLength - edgeLength) / edgeLength).max(0.0)).eval();

        // store current points positions
        stopCriterionBuffer = points;

        // move points
        for (int edge = 0; edge < edgeIndices.rows(); ++edge) {
            if (edgeIndices(edge, 0) >= fixedPoints.rows()) {
                points.row(edgeIndices(edge, 0)) += constants::deltaT * forceVector.row(edge);
            }
            if (edgeIndices(edge, 1) >= fixedPoints.rows()) {
                points.row(edgeIndices(edge, 1)) -= constants::deltaT * forceVector.row(edge);
            }
        }

        // project points outside of domain to boundary
        utils::projectPointsToBoundary(distanceFunction, initialPointDistance, points);

        // stop, when maximum points movement is below threshold
        if ((points - stopCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() <
            constants::pointsMovementThreshold * initialPointDistance) {
            break;
        }
    }

    return std::make_tuple(points, triangulation);
}

bool mrpt::vision::pnp::CPnP::dls(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
        bool ret = d.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return ret;

        #else
        throw(-1)
        #endif
    }
    catch(int e)
    {
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}