Beispiel #1
0
void Viewer::render(const WorldModel& wm)
{
    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

    if (!cam_initialized_)
    {
        if (!wm.points().empty() || !wm.vertices().empty())
        {
            geo::Vec3 p_total(0, 0, 0);
            for(unsigned int i = 0; i < wm.points().size(); ++i)
                p_total += wm.points()[i];

            for(unsigned int i = 0; i < wm.vertices().size(); ++i)
                p_total += wm.vertices()[i];

            cam_control_.cam_lookat = p_total / (wm.points().size() + wm.vertices().size());
            cam_initialized_ = true;
        }
        else
        {
            cam_control_.cam_lookat = geo::Vec3(0, 0, 0);
        }

        cam_control_.cam_dist = 5;
        cam_control_.cam_pitch = 0.7;
        cam_control_.cam_yaw = 3.1415;
    }

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    // Calculate camera pose

    cam_control_.cam_pose.t = geo::Vector3(cos(cam_control_.cam_yaw), sin(cam_control_.cam_yaw), 0)
                                * cos(cam_control_.cam_pitch) * cam_control_.cam_dist;
    cam_control_.cam_pose.t.z = sin(cam_control_.cam_pitch) * cam_control_.cam_dist;
    cam_control_.cam_pose.t += cam_control_.cam_lookat;

    geo::Vec3 rz = -(cam_control_.cam_lookat - cam_control_.cam_pose.t).normalized();
    geo::Vec3 rx = geo::Vector3(0, 0, 1).cross(rz).normalized();
    geo::Vec3 ry = rz.cross(rx).normalized();

    cam_control_.cam_pose.R = geo::Mat3(rx.x, ry.x, rz.x,
                                        rx.y, ry.y, rz.y,
                                        rx.z, ry.z, rz.z);

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

    canvas_.setTo(cv::Vec3b(20, 20, 20));
    cv::Mat depth_image(canvas_.rows, canvas_.cols, CV_32FC1, 0.0);

    LightingRenderer res(depth_image, canvas_, cam_control_.cam_pose);
    mwm::render::renderDepth(wm, P_, cam_control_.cam_pose, res);

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

    geo::Pose3D sensor_pose_inv = cam_control_.cam_pose.inverse();

    for(unsigned int i = 0; i < wm.points().size(); ++i)
    {
        const geo::Vec3& p = wm.points()[i];
        const cv::Vec3b& color = wm.point_colors()[i];

        geo::Vec3 p_sensor = sensor_pose_inv * p;
        geo::Vec2i p_2d = P_.project3Dto2D(p_sensor);
        double z = -p_sensor.z;

//        if (z < 0)
//            continue;

        if (p_2d.x < 0 || p_2d.y < 0 || p_2d.x >= canvas_.cols || p_2d.y >= canvas_.rows)
            continue;

        float& d = depth_image.at<float>(p_2d.y, p_2d.x);
        if (d == 0 || z < d)
        {
            d = z;
            canvas_.at<cv::Vec3b>(p_2d.y, p_2d.x) = color;
//            cv::circle(canvas_, cv::Point(p_2d.x, p_2d.y), 3, cv::Scalar(color[0], color[1], color[2]), CV_FILLED);
        }
    }

//    unsigned int size = canvas_.rows * canvas_.cols;
//    for(unsigned int i = 0; i < size; ++i)
//    {
//        float d = depth_image.at<float>(i);
//        if (d == 0)
//            canvas_.at<cv::Vec3b>(i) = cv::Vec3b(20, 20, 20);
//        else
//            canvas_.at<cv::Vec3b>(i) = (d / 10) * cv::Vec3b(255, 255, 255);
//    }
}
Beispiel #2
0
void renderDepth(const WorldModel& wm, const ProjectionMatrix& P, const geo::Pose3D& sensor_pose, Result& res)
{
    // Parameters
    double near_clip_z = -0.1;

    // - - - - - - - - - - - - - - - - - - - - - - - - - - - -

    const std::vector<Triangle>& triangles = wm.triangles();
    const std::vector<geo::Vec3>& vertices = wm.vertices();

    // transform points
    std::vector<geo::Vec3> vertices_t(vertices.size());
    std::vector<geo::Vec2i> vertices_proj(vertices.size());

    geo::Pose3D sensor_pose_inv = sensor_pose.inverse();

    for(unsigned int i = 0; i < vertices.size(); ++i)
    {
        vertices_t[i] = sensor_pose_inv * vertices[i];
        vertices_proj[i] = P.project3Dto2D(vertices_t[i]);
    }

    for(const auto& t : triangles)
    {
        const geo::Vec3& p1_3d = vertices_t[t.i1];
        const geo::Vec3& p2_3d = vertices_t[t.i2];
        const geo::Vec3& p3_3d = vertices_t[t.i3];

        res.triangleHook(t, p1_3d, p2_3d, p3_3d);

        int n_verts_in = 0;
        bool v1_in = false;
        bool v2_in = false;
        bool v3_in = false;
        const geo::Vec3* vIn[3];

        if (p1_3d.z < near_clip_z) {
            ++n_verts_in;
            v1_in = true;
        }

        if (p2_3d.z < near_clip_z) {
            ++n_verts_in;
            v2_in = true;
        }

        if (p3_3d.z < near_clip_z) {
            ++n_verts_in;
            v3_in = true;
        }

        if (n_verts_in == 1)
        {
            if (v1_in) { vIn[0] = &(p1_3d); vIn[1] = &(p2_3d); vIn[2] = &(p3_3d); }
            if (v2_in) { vIn[0] = &(p2_3d); vIn[1] = &(p3_3d); vIn[2] = &(p1_3d); }
            if (v3_in) { vIn[0] = &(p3_3d); vIn[1] = &(p1_3d); vIn[2] = &(p2_3d); }

            //Parametric line stuff
            // p = v0 + v01*t
            geo::Vec3 v01 = *vIn[1] - *vIn[0];

            float t1 = ((near_clip_z - (*vIn[0]).z) / v01.z );

            geo::Vec3 new2(vIn[0]->x + v01.x * t1, vIn[0]->y + v01.y * t1, near_clip_z);

            // Second vert point
            geo::Vec3 v02 = *vIn[2] - *vIn[0];

            float t2 = ((near_clip_z - (*vIn[0]).z) / v02.z);

            geo::Vec3 new3(vIn[0]->x + v02.x * t2, vIn[0]->y + v02.y * t2, near_clip_z);

            drawTriangle(*vIn[0], new2, new3, P, res);
        }
        else if (n_verts_in == 2)
        {
            if (!v1_in) { vIn[0]=&(p2_3d); vIn[1]=&(p3_3d); vIn[2]=&(p1_3d); }
            if (!v2_in) { vIn[0]=&(p3_3d); vIn[1]=&(p1_3d); vIn[2]=&(p2_3d); }
            if (!v3_in) { vIn[0]=&(p1_3d); vIn[1]=&(p2_3d); vIn[2]=&(p3_3d); }

            //Parametric line stuff
            // p = v0 + v01*t
            geo::Vec3 v01 = *vIn[2] - *vIn[0];

            float t1 = ((near_clip_z - (*vIn[0]).z)/v01.z );

            geo::Vec3 new2((*vIn[0]).x + v01.x * t1,(*vIn[0]).y + v01.y * t1, near_clip_z);

            // Second point
            geo::Vec3 v02 = *vIn[2] - *vIn[1];

            float t2 = ((near_clip_z - (*vIn[1]).z)/v02.z);

            geo::Vec3 new3((*vIn[1]).x + v02.x * t2, (*vIn[1]).y + v02.y * t2, near_clip_z);

            drawTriangle(*vIn[0], *vIn[1], new2, P, res);

            drawTriangle(new2, *vIn[1], new3, P, res);

        }
        else if (n_verts_in == 3)
        {
            const geo::Vec2i& p1_2d = vertices_proj[t.i1];
            const geo::Vec2i& p2_2d = vertices_proj[t.i2];
            const geo::Vec2i& p3_2d = vertices_proj[t.i3];

            drawTriangle2D(geo::Vec3f(p1_2d.x, p1_2d.y, 1.0f / -p1_3d.z),
                           geo::Vec3f(p2_2d.x, p2_2d.y, 1.0f / -p2_3d.z),
                           geo::Vec3f(p3_2d.x, p3_2d.y, 1.0f / -p3_3d.z),
                           res);
        }
    }
}