Ejemplo n.º 1
0
bool mrpt::vision::pnp::CPnP::so3(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{
        // 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;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::p3p p(cam_in_eig);
        p.solve(R,t, obj_pts_eig, img_pts_eig);

        mrpt::vision::pnp::so3 s(obj_pts_eig, img_pts_eig, cam_in_eig, n);
        bool ret = s.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat<<t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            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;
    }
}
Ejemplo n.º 2
0
void softmax<T>::compute_gradient(Eigen::Ref<const EigenMat> const &train,
                                  Eigen::Ref<const EigenMat> const &weight,
                                  Eigen::Ref<const EigenMat> const &ground_truth)
{
    grad_.noalias() =
            (ground_truth.array() - hypothesis_.array())
            .matrix() * train.transpose();
    auto const NSamples = static_cast<double>(train.cols());
    grad_.array() = grad_.array() / -NSamples +
            params_.lambda_ * weight.array();
}
Ejemplo n.º 3
0
int softmax<T>::predict(Eigen::Ref<const EigenMat> const &input)
{    
    CV_Assert(input.cols() == 1);
    compute_hypothesis(input, weight_);
    probability_ = (hypothesis_ * input.transpose()).
            rowwise().sum();
    EigenMat::Index max_row = 0, max_col = 0;
    probability_.maxCoeff(&max_row, &max_col);

    return max_row;
}
Ejemplo n.º 4
0
 void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
 {
     out = x.transpose().normalized();
 }
Ejemplo n.º 5
0
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;
    }
}