C++ Pose3D::cvTranslation方法代码示例

void MeshRenderer :: setPose(const Pose3D& pose, float* arg_near_plane, float* arg_far_plane)
{
    VertexBufferObject& vbo = m_vertex_buffer_object;
    pose.cvCameraTransform().copyTo(vbo.model_view_matrix);
    // Transpose the matrix for OpenGL column-major.
    vbo.model_view_matrix = vbo.model_view_matrix.t();

    if (!(arg_near_plane && arg_far_plane))
    {
        estimateOptimalPlanes(pose, &m_last_near_plane, &m_last_far_plane);
    }
    else
    {
        m_last_near_plane = *arg_near_plane;
        m_last_far_plane = *arg_far_plane;
    }

    m_pbuffer->makeCurrent();
    glMatrixMode (GL_MODELVIEW);
    glLoadIdentity ();
    cv::Vec3f euler_angles = pose.cvEulerRotation();
    glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]);
    glRotatef(euler_angles[2]*180.0/M_PI, 0, 0, 1);
    glRotatef(euler_angles[1]*180.0/M_PI, 0, 1, 0);
    glRotatef(euler_angles[0]*180.0/M_PI, 1, 0, 0);

    glMatrixMode (GL_PROJECTION);
    glLoadIdentity ();
    double dx = pose.imageCenterX() - (m_pbuffer->width() / 2.0);
    double dy = pose.imageCenterY() - (m_pbuffer->height() / 2.0);
    glViewport(dx, -dy, m_pbuffer->width(), m_pbuffer->height());
    if (pose.isOrthographic())
    {
        ntk_dbg_print(pose.focalX()/2, 0);
        ntk_dbg_print(pose.focalY()/2, 0);
        glOrtho(-pose.focalX()/2, pose.focalX()/2, -pose.focalY()/2, pose.focalY()/2, m_last_near_plane, m_last_far_plane);
    }
    else
    {
        double fov = (180.0/M_PI) * 2.0*atan(m_pbuffer->height()/(2.0*pose.focalY()));
        // double fov2 = (180.0/M_PI) * 2.0*atan(image.cols/(2.0*pose.focalX()));
        // ntk_dbg_print(fov2, 2);
        // gluPerspective(fov2,  double(image.rows)/image.cols, near_plane, far_plane);
        gluPerspective(fov, double(m_pbuffer->width())/m_pbuffer->height(), m_last_near_plane, m_last_far_plane);
    }

    glMatrixMode (GL_MODELVIEW);
}

void CalibrationMeshViewer::onCameraPositionUpdate(const cv::Vec3f &translation, const cv::Vec3f &rotation)
{
    if (!m_calibration_mode)
    {
        MeshViewer::onCameraPositionUpdate(translation, rotation);
        return;
    }

    GLdouble m[16];
    GLdouble deltam[16];

    const float rotation_scale = 0.2;
    const float translation_scale = 0.2;

    // Get the delta transformation is visualization frame.
    makeCurrent();
    glMatrixMode(GL_MODELVIEW);
    glGetDoublev(GL_MODELVIEW_MATRIX, m);
    glLoadIdentity();
    glTranslatef(translation_scale*translation[0],translation_scale*translation[1],translation_scale*translation[2]);
    glTranslatef(m_display_center.x,m_display_center.y,m_display_center.z);
    glRotatef(rotation_scale*rotation[0], 0,1,0);
    glRotatef(rotation_scale*rotation[1], 1,0,0);
    glTranslatef(-m_display_center.x,-m_display_center.y,-m_display_center.z);
    glGetDoublev(GL_MODELVIEW_MATRIX, deltam);
    glLoadMatrixd(m);

    cv::Vec3f t,r;
    window->getCalibration(t, r);
    Pose3D p_old;
    p_old.applyTransformBefore(t, r);

    cv::Mat1d H_old = p_old.cvCameraTransformd();
    cv::Mat1d H(4,4,(double*)deltam); H = H.t(); // delta rotation AFTER model view matrix
    cv::Mat1d M(4,4,(double*)m); M = M.t(); // model view matrix

    cv::Mat1d Hp = (M.inv() * H * M * H_old.inv()).inv(); // delta rotation BEFORE model view matrix

    Pose3D p;
    p.setCameraTransform(Hp);

    window->updateFromCalibration(p.cvTranslation(), p.cvEulerRotation());
    window->updateToCalibration();
}

void MeshViewer :: addMeshToDisplayList(const ntk::Mesh& mesh, const Pose3D& pose, MeshViewerMode mode)
{
    int new_list_index = glGenLists(1);
    glNewList(new_list_index, GL_COMPILE);
    if (mesh.texture.data)
    {
        // Last texture id was just created
        GLuint texture = m_upcoming_textures[m_upcoming_textures.size()-1];
        glEnable(GL_TEXTURE_2D);
        glBindTexture( GL_TEXTURE_2D, texture );
    }
    else
    {
        glDisable(GL_TEXTURE_2D);
    }
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]);
    Vec3f euler_angles = pose.cvEulerRotation();
    glRotatef(rad_to_deg(euler_angles[0]), 1, 0, 0);
    glRotatef(rad_to_deg(euler_angles[1]), 0, 1, 0);
    glRotatef(rad_to_deg(euler_angles[2]), 0, 0, 1);

    if (mode & WIREFRAME)
    {
        glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
    }
    else
    {
        glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
    }

    int64 point_start_time = ntk::Time::getMillisecondCounter();
    if (mesh.faces.size() == 0)
    {
        glBegin(GL_POINTS);
        for (int i = 0; i < mesh.vertices.size(); ++i)
        {
            const Point3f& v = mesh.vertices[i];
            // if (i % 1000 == 0)
            // ntk_dbg_print(v, 1);
            if (mesh.hasColors())
                glColor3f(mesh.colors[i][0]/255.0, mesh.colors[i][1]/255.0, mesh.colors[i][2]/255.0);
            glVertex3f(v.x, v.y, v.z);
        }
        glEnd();
    }
    int64 point_end_time = ntk::Time::getMillisecondCounter();
    ntk_dbg_print(point_end_time-point_start_time, 1);

    {
        glBegin(GL_TRIANGLES);
        for (int i = 0; i < mesh.faces.size(); ++i)
        {
            int i1 = mesh.faces[i].indices[0];
            int i2 = mesh.faces[i].indices[1];
            int i3 = mesh.faces[i].indices[2];

            const Point3f& v1 = mesh.vertices[i1];
            const Point3f& v2 = mesh.vertices[i2];
            const Point3f& v3 = mesh.vertices[i3];

            Vec3f nm = (Vec3f(v2-v1).cross(v3-v2));
            normalize(nm);

            if (!mesh.hasColors())
                glColor3f(1.0f, 0.0f, 0.0f);

            if (mesh.hasColors())
                glColor3f(mesh.colors[i1][0]/255.0, mesh.colors[i1][1]/255.0, mesh.colors[i1][2]/255.0);
            if (mesh.hasTexcoords())
                glTexCoord2f(mesh.texcoords[i1].x, mesh.texcoords[i1].y);
            glVertex3f(v1.x, v1.y, v1.z);
            glNormal3f(nm[0], nm[1], nm[2]);

            if (mesh.hasColors())
                glColor3f(mesh.colors[i2][0]/255.0, mesh.colors[i2][1]/255.0, mesh.colors[i2][2]/255.0);
            if (mesh.hasTexcoords())
                glTexCoord2f(mesh.texcoords[i2].x, mesh.texcoords[i2].y);
            glVertex3f(v2.x, v2.y, v2.z);
            glNormal3f(nm[0], nm[1], nm[2]);

            if (mesh.hasColors())
                glColor3f(mesh.colors[i3][0]/255.0, mesh.colors[i3][1]/255.0, mesh.colors[i3][2]/255.0);
            if (mesh.hasTexcoords())
                glTexCoord2f(mesh.texcoords[i3].x, mesh.texcoords[i3].y);
            glVertex3f(v3.x, v3.y, v3.z);
            glNormal3f(nm[0], nm[1], nm[2]);
        }
        glEnd();
    }
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();
    glEndList();

    m_upcoming_display_lists.push_back(new_list_index);
}