LVRCamDataItem.cpp

Go to the documentation of this file.

#include "LVRCamDataItem.hpp"
#include "LVRModelItem.hpp"
#include "LVRItemTypes.hpp"
#include "LVRScanDataItem.hpp"
 
#include <vtkVersion.h>
#include <vtkFrustumSource.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkSmartPointer.h>
#include <vtkCamera.h>
#include <vtkPlanes.h>
#include <vtkMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkPolyDataMapper.h>
#include <vtkTriangle.h>
#include <vtkCellArray.h>
#include <vtkMatrix4x4.h>
#include <vtkTransform.h>
 
#include "lvr2/types/MatrixTypes.hpp"
 
namespace lvr2
{
 
LVRCamDataItem::LVRCamDataItem(
    ScanImage& data,
    std::shared_ptr<ScanDataManager> sdm,
    size_t cam_id,
    vtkSmartPointer<vtkRenderer> renderer,
    QString name,
    QTreeWidgetItem *parent
)
: QTreeWidgetItem(parent, LVRCamDataItemType)
{
    m_pItem  = nullptr;
    m_cvItem = nullptr;
    m_data   = data;
    m_name   = name;
    m_sdm    = sdm;
    m_cam_id  = cam_id;
    m_renderer = renderer;
 
    // m_matrix = m_data.m_extrinsics;
 
    // change this to not inverse
    bool dummy;
    m_matrix = m_data.extrinsics;//.inv(dummy);
 
    // set Transform from 
    setTransform(m_matrix);
 
    // TODO: How do we get the intrincs into the item
    // using the new structure??
    m_intrinsics = Intrinsicsd::Identity();
    
 
    // init pose
    double pose[6];
    extrinsicsToEuler<double>(m_data.extrinsics, pose);
 
    m_pose.x = pose[0];
    m_pose.y = pose[1];
    m_pose.z = pose[2];
    m_pose.r = pose[3]  * 57.295779513;
    m_pose.t = pose[4]  * 57.295779513;
    m_pose.p = pose[5]  * 57.295779513;
 
    m_pItem = new LVRPoseItem(ModelBridgePtr(new LVRModelBridge( ModelPtr( new Model))), this);
 
    m_cvItem = new LVRCvImageItem(sdm, renderer, "Image", this);
 
    m_pItem->setPose(m_pose);
 
    m_frustrum_actor = genFrustrum(0.1);
    
    // load data
    reload(renderer);
 
    setText(0, m_name);
    setCheckState(0, Qt::Unchecked );
}
 
void LVRCamDataItem::reload(vtkSmartPointer<vtkRenderer> renderer)
{
    
}
 
void LVRCamDataItem::setVisibility(bool visible)
{
    if(checkState(0) && visible)
    {
        std::cout << "ADD ACTOR" << std::endl;
        m_renderer->AddActor(m_frustrum_actor);
    } else {
        m_renderer->RemoveActor(m_frustrum_actor);
    }
}
 
Transformd LVRCamDataItem::getGlobalTransform()
{
    Transformd ret = m_matrix;
    QTreeWidgetItem* parent_it = parent();
 
    while(parent_it != NULL)
    {
 
        auto transform_obj = dynamic_cast< Transformable* >(parent_it);
        if(transform_obj)
        {
            ret = ret * transform_obj->getTransform();
        }
        parent_it = parent_it->parent();
    }
 
    return ret;
}
 
void LVRCamDataItem::setCameraView()
{
    auto cam = m_renderer->GetActiveCamera();
 
    double x,y,z;
    cam->GetPosition(x,y,z);
 
    Transformd T = getGlobalTransform();
 
    // T.transpose();
 
 
    Vector3d cam_origin(0.0, 0.0, -1.0);
    Vector3d view_up(1.0, 0.0, 0.0);
    Vector3d focal_point(0.0, 0.0, 0.0);
 
 
    cam_origin = lvr2::multiply(T, cam_origin);
    view_up = lvr2::multiply(T, view_up);
    focal_point = lvr2::multiply(T, focal_point);
 
    cam->SetPosition(cam_origin.x(), cam_origin.y(), cam_origin.z());
    cam->SetFocalPoint(focal_point.x(), focal_point.y(), focal_point.z());
    cam->SetViewUp(view_up.x(), view_up.y(), view_up.z());
 
    //  TODO: set intrinsics
 
}
 
std::vector<Vector3d > LVRCamDataItem::genFrustrumLVR(float scale)
{
    Transformd T = getGlobalTransform().transpose();
    
    
 
    std::vector<Vector3d > cv_pixels;
 
    // TODO change this. get size of image
 
    Intrinsicsd intrisics_corrected = Intrinsicsd::Identity();
 
    intrisics_corrected(0,0) = m_intrinsics(1,1);
    intrisics_corrected(1,1) = m_intrinsics(0,0);
    intrisics_corrected(0,2) = m_intrinsics(1,2);
    intrisics_corrected(1,2) = m_intrinsics(0,2);
 
    int u_max = intrisics_corrected(0, 2) * 2;
    int v_max = intrisics_corrected(1, 2) * 2;
 
    Intrinsicsd cam_mat_inv = intrisics_corrected.inverse();
    
 
    std::cout << "u,v max: "<< u_max << "," << v_max << std::endl;
    // std::cout << v_max << std::endl;
 
    // opencv x,y,z
    // top left
    cv_pixels.push_back({0.0, 0.0, 1.0});
    // bottom left
    cv_pixels.push_back({0.0, double(v_max), 1.0});
    // bottem right
    cv_pixels.push_back({double(u_max), double(v_max), 1.0});
    // top right
    cv_pixels.push_back({double(u_max), 0.0, 1.0});
 
 
    // generate frustrum points
    std::vector<Vector3d > lvr_points;
 
 
    // origin
    lvr_points.push_back({0.0, 0.0, 0.0});
 
    std::cout << "cam space: " << std::endl;
    for(int i=0; i<cv_pixels.size(); i++)
    {
        Vector3d pixel = cv_pixels[i];
        //Vector3d p = lvr2::multiply(cam_mat_inv, pixel);
 
        Vector3d p = cam_mat_inv * pixel;
 
        // Vector3d p = pixel;
 
 
        Vector3d tmp = openCvToLvr(p);
 
        std::cout << tmp.transpose() << std::endl;
 
        tmp *= scale;
        lvr_points.push_back(tmp);
    }
 
    std::cout << "world space:" << std::endl;
    // transform frustrum
    for(int i=0; i<lvr_points.size(); i++)
    {
        // lvr_points[i] = lvr2::multiply(T, lvr_points[i]);
        std::cout << lvr_points[i].transpose() << std::endl;
        // lvr_points[i] = lvr_points[i];
    }
 
    return lvr_points;
}
 
vtkSmartPointer<vtkActor> LVRCamDataItem::genFrustrum(float scale)
{
 
    std::vector<Vector3d > lvr_points = genFrustrumLVR(scale);
 
    // Setup points
    vtkSmartPointer<vtkPoints> points =
        vtkSmartPointer<vtkPoints>::New();
 
    // convert to vtk
    points->SetNumberOfPoints(lvr_points.size());
 
    for(int i=0; i<lvr_points.size(); i++)
    {
        auto p = lvr_points[i];
        points->SetPoint(i, p.x(), p.y(), p.z());
    }
 
    // // Define some colors
    unsigned char white[3] = {255, 255, 255}; // origin
    unsigned char red[3] = {255, 0, 0}; // top left
    unsigned char green[3] = {0, 255, 0}; // bottom left
    unsigned char blue[3] = {0, 0, 255}; // bottom right
    unsigned char yellow[3] = {255, 255, 0}; // top right
 
    // // Setup the colors array
    vtkSmartPointer<vtkUnsignedCharArray> colors =
        vtkSmartPointer<vtkUnsignedCharArray>::New();
    colors->SetNumberOfComponents(3);
    colors->SetNumberOfTuples(lvr_points.size());
    colors->SetName("Colors");
 
#if VTK_MAJOR_VERSION < 7
    colors->SetTupleValue(0, white);
    colors->SetTupleValue(1, red);
    colors->SetTupleValue(2, green);
    colors->SetTupleValue(3, blue);
    colors->SetTupleValue(4, yellow);
#else
    colors->SetTypedTuple(0, white); // no idea how the new method is called
    colors->SetTypedTuple(1, red);
    colors->SetTypedTuple(2, green);
    colors->SetTypedTuple(3, blue);
    colors->SetTypedTuple(4, yellow);
#endif
 
    // Create a triangle
    vtkSmartPointer<vtkCellArray> triangles =
        vtkSmartPointer<vtkCellArray>::New();
 
    
    vtkSmartPointer<vtkTriangle> triangle =
        vtkSmartPointer<vtkTriangle>::New();
 
    // left plane
 
    triangle->GetPointIds()->SetId(0, 0);
    triangle->GetPointIds()->SetId(1, 1);
    triangle->GetPointIds()->SetId(2, 2);
 
    triangles->InsertNextCell(triangle);
 
    // bottom plane
    triangle->GetPointIds()->SetId(0, 0);
    triangle->GetPointIds()->SetId(1, 2);
    triangle->GetPointIds()->SetId(2, 3);
 
    triangles->InsertNextCell(triangle);
 
    // right plane
    triangle->GetPointIds()->SetId(0, 0);
    triangle->GetPointIds()->SetId(1, 3);
    triangle->GetPointIds()->SetId(2, 4);
 
    triangles->InsertNextCell(triangle);
 
    // top plane
    triangle->GetPointIds()->SetId(0, 0);
    triangle->GetPointIds()->SetId(1, 4);
    triangle->GetPointIds()->SetId(2, 1);
 
    triangles->InsertNextCell(triangle);
 
    // Create a polydata object and add everything to it
    vtkSmartPointer<vtkPolyData> polydata =
        vtkSmartPointer<vtkPolyData>::New();
    polydata->SetPoints(points);
    polydata->SetPolys(triangles);
    polydata->GetPointData()->SetScalars(colors);
 
    // // Visualize
    vtkSmartPointer<vtkPolyDataMapper> mapper =
        vtkSmartPointer<vtkPolyDataMapper>::New();
#if VTK_MAJOR_VERSION <= 5
    mapper->SetInputConnection(polydata->GetProducerPort());
#else
    mapper->SetInputData(polydata);
#endif
    vtkSmartPointer<vtkActor> actor =
        vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(mapper);
    return actor;
}
 
LVRCamDataItem::~LVRCamDataItem()
{
    // we don't want to do delete m_bbItem, m_pItem and m_pcItem here
    // because QTreeWidgetItem deletes its childs automatically in its destructor.
}
 
} // namespace lvr2