#include "precomp.hpp"
#include "viz3d_impl.hpp"

#include <vtkRenderWindowInteractor.h>
#ifndef __APPLE__
vtkRenderWindowInteractor* vtkRenderWindowInteractorFixNew ()
{
  return vtkRenderWindowInteractor::New();
}
#endif

/////////////////////////////////////////////////////////////////////////////////////////////
cv::viz::Viz3d::VizImpl::VizImpl (const std::string &name)
    :  style_ (vtkSmartPointer<cv::viz::InteractorStyle>::New ())
    , cloud_actor_map_ (new CloudActorMap)
    , shape_actor_map_ (new ShapeActorMap)
    , widget_actor_map_ (new WidgetActorMap)
    , s_lastDone_(0.0)
{
    renderer_ = vtkSmartPointer<vtkRenderer>::New ();

    // Create a RendererWindow
    window_ = vtkSmartPointer<vtkRenderWindow>::New ();

    // Set the window size as 1/2 of the screen size
    cv::Vec2i window_size = cv::Vec2i(window_->GetScreenSize()) / 2;
    window_->SetSize (window_size.val);

    window_->AddRenderer (renderer_);

    // Create the interactor style
    style_->Initialize ();
    style_->setRenderer (renderer_);
    style_->setCloudActorMap (cloud_actor_map_);
    style_->UseTimersOn ();

    /////////////////////////////////////////////////
    interactor_ = vtkSmartPointer <vtkRenderWindowInteractor>::Take (vtkRenderWindowInteractorFixNew ());

    //win_->PointSmoothingOn ();
    //win_->LineSmoothingOn ();
    //win_->PolygonSmoothingOn ();
    window_->AlphaBitPlanesOff ();
    window_->PointSmoothingOff ();
    window_->LineSmoothingOff ();
    window_->PolygonSmoothingOff ();
    window_->SwapBuffersOn ();
    window_->SetStereoTypeToAnaglyph ();

    interactor_->SetRenderWindow (window_);
    interactor_->SetInteractorStyle (style_);
    //interactor_->SetStillUpdateRate (30.0);
    interactor_->SetDesiredUpdateRate (30.0);

    // Initialize and create timer, also create window
    interactor_->Initialize ();
    timer_id_ = interactor_->CreateRepeatingTimer (5000L);

    // Set a simple PointPicker
    vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
    pp->SetTolerance (pp->GetTolerance () * 2);
    interactor_->SetPicker (pp);

    exit_main_loop_timer_callback_ = vtkSmartPointer<ExitMainLoopTimerCallback>::New ();
    exit_main_loop_timer_callback_->viz_ = this;
    exit_main_loop_timer_callback_->right_timer_id = -1;
    interactor_->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);

    exit_callback_ = vtkSmartPointer<ExitCallback>::New ();
    exit_callback_->viz_ = this;
    interactor_->AddObserver (vtkCommand::ExitEvent, exit_callback_);

    resetStoppedFlag ();


    //////////////////////////////

    String window_name("Viz");
    window_name = name.empty() ? window_name : window_name + " - " + name;
    window_->SetWindowName (window_name.c_str ());
}

/////////////////////////////////////////////////////////////////////////////////////////////
cv::viz::Viz3d::VizImpl::~VizImpl ()
{
    if (interactor_ != NULL)
        interactor_->DestroyTimer (timer_id_);

    renderer_->Clear();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::saveScreenshot (const std::string &file) { style_->saveScreenshot (file); }

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::registerMouseCallback(MouseCallback callback, void* cookie)
{ style_->registerMouseCallback(callback, cookie); }

void cv::viz::Viz3d::VizImpl::registerKeyboardCallback(KeyboardCallback callback, void* cookie)
{ style_->registerKeyboardCallback(callback, cookie); }

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::spin ()
{
    resetStoppedFlag ();
    window_->Render ();
    interactor_->Start ();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::spinOnce (int time, bool force_redraw)
{
    resetStoppedFlag ();

    if (time <= 0)
        time = 1;

    if (force_redraw)
        interactor_->Render ();

    double s_now_ = cv::getTickCount() / cv::getTickFrequency();
    if (s_lastDone_ > s_now_)
      s_lastDone_ = s_now_;

    if ((s_now_ - s_lastDone_) > (1.0 / interactor_->GetDesiredUpdateRate ()))
    {
        exit_main_loop_timer_callback_->right_timer_id = interactor_->CreateRepeatingTimer (time);
        interactor_->Start ();
        interactor_->DestroyTimer (exit_main_loop_timer_callback_->right_timer_id);
        s_lastDone_ = s_now_;
    }
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removePointCloud (const std::string &id)
{
    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
    if (am_it == cloud_actor_map_->end ())
        return false;

    if (removeActorFromRenderer (am_it->second.actor))
        return cloud_actor_map_->erase (am_it), true;

    return false;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeShape (const std::string &id)
{
    // Check to see if the given ID entry exists
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    // Extra step: check if there is a cloud with the same ID
    CloudActorMap::iterator ca_it = cloud_actor_map_->find (id);

    bool shape = true;
    // Try to find a shape first
    if (am_it == shape_actor_map_->end ())
    {
        // There is no cloud or shape with this ID, so just exit
        if (ca_it == cloud_actor_map_->end ())
            return false;
        // Cloud found, set shape to false
        shape = false;
    }

    // Remove the pointer/ID pair to the global actor map
    if (shape)
    {
        if (removeActorFromRenderer (am_it->second))
        {
            shape_actor_map_->erase (am_it);
            return (true);
        }
    }
    else
    {
        if (removeActorFromRenderer (ca_it->second.actor))
        {
            cloud_actor_map_->erase (ca_it);
            return true;
        }
    }
    return false;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeText3D (const std::string &id)
{
    // Check to see if the given ID entry exists
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it == shape_actor_map_->end ())
        return false;

    // Remove it from all renderers
    if (removeActorFromRenderer (am_it->second))
        return shape_actor_map_->erase (am_it), true;

    return false;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeAllPointClouds ()
{
    // Check to see if the given ID entry exists
    CloudActorMap::iterator am_it = cloud_actor_map_->begin ();
    while (am_it != cloud_actor_map_->end () )
    {
        if (removePointCloud (am_it->first))
            am_it = cloud_actor_map_->begin ();
        else
            ++am_it;
    }
    return (true);
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeAllShapes ()
{
    // Check to see if the given ID entry exists
    ShapeActorMap::iterator am_it = shape_actor_map_->begin ();
    while (am_it != shape_actor_map_->end ())
    {
        if (removeShape (am_it->first))
            am_it = shape_actor_map_->begin ();
        else
            ++am_it;
    }
    return (true);
}


//////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor)
{
    vtkLODActor* actor_to_remove = vtkLODActor::SafeDownCast (actor);



    // Iterate over all actors in this renderer
    vtkPropCollection* actors = renderer_->GetViewProps ();
    actors->InitTraversal ();

    vtkProp* current_actor = NULL;
    while ((current_actor = actors->GetNextProp ()) != NULL)
    {
        if (current_actor != actor_to_remove)
            continue;
        renderer_->RemoveActor (actor);
        //        renderer->Render ();
        // Found the correct viewport and removed the actor
        return (true);
    }

    return false;
}

//////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor)
{
    vtkActor* actor_to_remove = vtkActor::SafeDownCast (actor);

    // Add it to all renderers
    //rens_->InitTraversal ();


        // Iterate over all actors in this renderer
    vtkPropCollection* actors = renderer_->GetViewProps ();
    actors->InitTraversal ();
    vtkProp* current_actor = NULL;
    while ((current_actor = actors->GetNextProp ()) != NULL)
    {
        if (current_actor != actor_to_remove)
            continue;
        renderer_->RemoveActor (actor);
        //        renderer->Render ();
        // Found the correct viewport and removed the actor
        return (true);
    }
    return false;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor)
{
    vtkProp* actor_to_remove = vtkProp::SafeDownCast(actor);

    vtkPropCollection* actors = renderer_->GetViewProps ();
    actors->InitTraversal ();
    vtkProp* current_actor = NULL;
    while ((current_actor = actors->GetNextProp ()) != NULL)
    {
        if (current_actor != actor_to_remove)
            continue;
        renderer_->RemoveActor (actor);
        return true;
    }
    return false;
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data, vtkSmartPointer<vtkLODActor> &actor, bool use_scalars)
{
    if (!actor)
        actor = vtkSmartPointer<vtkLODActor>::New ();

    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
    mapper->SetInput (data);

    if (use_scalars)
    {
        vtkSmartPointer<vtkDataArray> scalars = data->GetPointData ()->GetScalars ();
        if (scalars)
        {
            cv::Vec3d minmax(scalars->GetRange());
            mapper->SetScalarRange(minmax.val);
            mapper->SetScalarModeToUsePointData ();

            // interpolation OFF, if data is a vtkPolyData that contains only vertices, ON for anything else.
            vtkPolyData* polyData = vtkPolyData::SafeDownCast (data);
            bool interpolation = (polyData && polyData->GetNumberOfCells () != polyData->GetNumberOfVerts ());

            mapper->SetInterpolateScalarsBeforeMapping (interpolation);
            mapper->ScalarVisibilityOn ();
        }
    }
    mapper->ImmediateModeRenderingOff ();

    actor->SetNumberOfCloudPoints (int (std::max<vtkIdType> (1, data->GetNumberOfPoints () / 10)));
    actor->GetProperty ()->SetInterpolationToFlat ();

    /// FIXME disabling backface culling due to known VTK bug: vtkTextActors are not
    /// shown when there is a vtkActor with backface culling on present in the scene
    /// Please see VTK bug tracker for more details: http://www.vtk.org/Bug/view.php?id=12588
    // actor->GetProperty ()->BackfaceCullingOn ();

    actor->SetMapper (mapper);
}

//////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setBackgroundColor (const Color& color)
{
    Color c = vtkcolor(color);
    renderer_->SetBackground (c.val);
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::getPointCloudRenderingProperties (int property, double &value, const std::string &id)
{
    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
    if (am_it == cloud_actor_map_->end ())
        return false;

    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);

    switch (property)
    {
        case VIZ_POINT_SIZE:
        {
            value = actor->GetProperty ()->GetPointSize ();
            actor->Modified ();
            break;
        }
        case VIZ_OPACITY:
        {
            value = actor->GetProperty ()->GetOpacity ();
            actor->Modified ();
            break;
        }
        case VIZ_LINE_WIDTH:
        {
            value = actor->GetProperty ()->GetLineWidth ();
            actor->Modified ();
            break;
        }
        default:
            CV_Assert("getPointCloudRenderingProperties: Unknown property");
    }

    return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::setPointCloudRenderingProperties (int property, double value, const std::string &id)
{
    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
    if (am_it == cloud_actor_map_->end ())
        return std::cout << "[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <" << id << ">!" << std::endl, false;

    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);

    switch (property)
    {
        case VIZ_POINT_SIZE:
        {
            actor->GetProperty ()->SetPointSize (float (value));
            actor->Modified ();
            break;
        }
        case VIZ_OPACITY:
        {
            actor->GetProperty ()->SetOpacity (value);
            actor->Modified ();
            break;
        }
            // Turn on/off flag to control whether data is rendered using immediate
            // mode or note. Immediate mode rendering tends to be slower but it can
            // handle larger datasets. The default value is immediate mode off. If you
            // are having problems rendering a large dataset you might want to consider
            // using immediate more rendering.
        case VIZ_IMMEDIATE_RENDERING:
        {
            actor->GetMapper ()->SetImmediateModeRendering (int (value));
            actor->Modified ();
            break;
        }
        case VIZ_LINE_WIDTH:
        {
            actor->GetProperty ()->SetLineWidth (float (value));
            actor->Modified ();
            break;
        }
        default:
            CV_Assert("setPointCloudRenderingProperties: Unknown property");
    }
    return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::setPointCloudSelected (const bool selected, const std::string &id)
{
    CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
    if (am_it == cloud_actor_map_->end ())
        return std::cout << "[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <" << id << ">!" << std::endl, false;

    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
    if (selected)
    {
        actor->GetProperty ()->EdgeVisibilityOn ();
        actor->GetProperty ()->SetEdgeColor (1.0, 0.0, 0.0);
        actor->Modified ();
    }
    else
    {
        actor->GetProperty ()->EdgeVisibilityOff ();
        actor->Modified ();
    }

    return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::setShapeRenderingProperties (int property, double value, const std::string &id)
{
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it == shape_actor_map_->end ())
        return std::cout << "[setShapeRenderingProperties] Could not find any shape with id <" << id << ">!\n" << std::endl, false;

    vtkActor* actor = vtkActor::SafeDownCast (am_it->second);

    switch (property)
    {
    case VIZ_POINT_SIZE:
    {
        actor->GetProperty ()->SetPointSize (float (value));
        actor->Modified ();
        break;
    }
    case VIZ_OPACITY:
    {
        actor->GetProperty ()->SetOpacity (value);
        actor->Modified ();
        break;
    }
    case VIZ_LINE_WIDTH:
    {
        actor->GetProperty ()->SetLineWidth (float (value));
        actor->Modified ();
        break;
    }
    case VIZ_FONT_SIZE:
    {
        vtkTextActor* text_actor = vtkTextActor::SafeDownCast (am_it->second);
        vtkSmartPointer<vtkTextProperty> tprop = text_actor->GetTextProperty ();
        tprop->SetFontSize (int (value));
        text_actor->Modified ();
        break;
    }
    case VIZ_REPRESENTATION:
    {
        switch (int (value))
        {
            case REPRESENTATION_POINTS:    actor->GetProperty ()->SetRepresentationToPoints (); break;
            case REPRESENTATION_WIREFRAME: actor->GetProperty ()->SetRepresentationToWireframe (); break;
            case REPRESENTATION_SURFACE:   actor->GetProperty ()->SetRepresentationToSurface ();  break;
        }
        actor->Modified ();
        break;
    }
    case VIZ_SHADING:
    {
        switch (int (value))
        {
        case SHADING_FLAT: actor->GetProperty ()->SetInterpolationToFlat (); break;
        case SHADING_GOURAUD:
        {
            if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetNormals ())
            {
                std::cout << "[cv::viz::PCLVisualizer::setShapeRenderingProperties] Normals do not exist in the dataset, but Gouraud shading was requested. Estimating normals...\n" << std::endl;

                vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New ();
                normals->SetInput (actor->GetMapper ()->GetInput ());
                normals->Update ();
                vtkDataSetMapper::SafeDownCast (actor->GetMapper ())->SetInput (normals->GetOutput ());
            }
            actor->GetProperty ()->SetInterpolationToGouraud ();
            break;
        }
        case SHADING_PHONG:
        {
            if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetNormals ())
            {
                std::cout << "[cv::viz::PCLVisualizer::setShapeRenderingProperties] Normals do not exist in the dataset, but Phong shading was requested. Estimating normals...\n" << std::endl;
                vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New ();
                normals->SetInput (actor->GetMapper ()->GetInput ());
                normals->Update ();
                vtkDataSetMapper::SafeDownCast (actor->GetMapper ())->SetInput (normals->GetOutput ());
            }
            actor->GetProperty ()->SetInterpolationToPhong ();
            break;
        }
        }
        actor->Modified ();
        break;
    }
    default:
        CV_Assert("setShapeRenderingProperties: Unknown property");

    }
    return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::initCameraParameters ()
{
    Camera camera_temp;
    // Set default camera parameters to something meaningful
    camera_temp.clip = Vec2d(0.01, 1000.01);

    // Look straight along the z-axis
    camera_temp.focal = Vec3d(0.0, 0.0, 1.0);

    // Position the camera at the origin
    camera_temp.pos = Vec3d(0.0, 0.0, 0.0);

    // Set the up-vector of the camera to be the y-axis
    camera_temp.view_up = Vec3d(0.0, 1.0, 0.0);

    // Set the camera field of view to about
    camera_temp.fovy = 0.8575;
    camera_temp.window_size = Vec2i(window_->GetScreenSize()) / 2;
    camera_temp.window_pos = Vec2i(0, 0);

    setCameraParameters (camera_temp);
}

/////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::cameraParamsSet () const { return (camera_set_); }

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::updateCamera ()
{
    std::cout << "[cv::viz::PCLVisualizer::updateCamera()] This method was deprecated, just re-rendering all scenes now." << std::endl;
    //rens_->InitTraversal ();
    // Update the camera parameters

    renderer_->Render ();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::getCameras (cv::viz::Camera& camera)
{
    vtkCamera* active_camera = renderer_->GetActiveCamera ();

    camera.pos = cv::Vec3d(active_camera->GetPosition());
    camera.focal = cv::Vec3d(active_camera->GetFocalPoint());
    camera.clip = cv::Vec2d(active_camera->GetClippingRange());
    camera.view_up = cv::Vec3d(active_camera->GetViewUp());

    camera.fovy = active_camera->GetViewAngle()/ 180.0 * CV_PI;
    camera.window_size = cv::Vec2i(renderer_->GetRenderWindow()->GetSize());
    camera.window_pos = cv::Vec2d::all(0);
}

/////////////////////////////////////////////////////////////////////////////////////////////
cv::Affine3f cv::viz::Viz3d::VizImpl::getViewerPose ()
{
    vtkCamera& camera = *renderer_->GetActiveCamera ();

    Vec3d pos(camera.GetPosition());
    Vec3d view_up(camera.GetViewUp());
    Vec3d focal(camera.GetFocalPoint());

    Vec3d y_axis = normalized(view_up);
    Vec3d z_axis = normalized(focal - pos);
    Vec3d x_axis = normalized(y_axis.cross(z_axis));

    cv::Matx33d R;
    R(0, 0) = x_axis[0];
    R(0, 1) = y_axis[0];
    R(0, 2) = z_axis[0];

    R(1, 0) = x_axis[1];
    R(1, 1) = y_axis[1];
    R(1, 2) = z_axis[1];

    R(2, 0) = x_axis[2];
    R(2, 1) = y_axis[2];
    R(2, 2) = z_axis[2];

    return cv::Affine3f(R, pos);
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::resetCamera ()
{
    renderer_->ResetCamera ();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setCameraPosition (const cv::Vec3d& pos, const cv::Vec3d& view, const cv::Vec3d& up)
{
    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetPosition (pos[0], pos[1], pos[2]);
    cam->SetFocalPoint (view[0], view[1], view[2]);
    cam->SetViewUp (up[0], up[1], up[2]);
    renderer_->Render ();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setCameraPosition (double pos_x, double pos_y, double pos_z, double up_x, double up_y, double up_z)
{
    //rens_->InitTraversal ();
    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetPosition (pos_x, pos_y, pos_z);
    cam->SetViewUp (up_x, up_y, up_z);
    renderer_->Render ();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setCameraParameters (const cv::Matx33f& intrinsics, const cv::Affine3f& extrinsics)
{
    // Position = extrinsic translation
    cv::Vec3f pos_vec = extrinsics.translation();


    // Rotate the view vector
    cv::Matx33f rotation = extrinsics.rotation();
    cv::Vec3f y_axis (0.f, 1.f, 0.f);
    cv::Vec3f up_vec (rotation * y_axis);

    // Compute the new focal point
    cv::Vec3f z_axis (0.f, 0.f, 1.f);
    cv::Vec3f focal_vec = pos_vec + rotation * z_axis;

    // Get the width and height of the image - assume the calibrated centers are at the center of the image
    Eigen::Vector2i window_size;
    window_size[0] = static_cast<int> (intrinsics(0, 2));
    window_size[1] = static_cast<int> (intrinsics(1, 2));

    // Compute the vertical field of view based on the focal length and image heigh
    double fovy = 2 * atan (window_size[1] / (2. * intrinsics (1, 1))) * 180.0 / M_PI;

    //rens_->InitTraversal ();


    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetPosition (pos_vec[0], pos_vec[1], pos_vec[2]);
    cam->SetFocalPoint (focal_vec[0], focal_vec[1], focal_vec[2]);
    cam->SetViewUp (up_vec[0], up_vec[1], up_vec[2]);
    cam->SetUseHorizontalViewAngle (0);
    cam->SetViewAngle (fovy);
    cam->SetClippingRange (0.01, 1000.01);
    window_->SetSize (window_size[0], window_size[1]);

    renderer_->Render ();
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setCameraParameters (const cv::viz::Camera &camera)
{
    //rens_->InitTraversal ();


    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetPosition (camera.pos[0], camera.pos[1], camera.pos[2]);
    cam->SetFocalPoint (camera.focal[0], camera.focal[1], camera.focal[2]);
    cam->SetViewUp (camera.view_up[0], camera.view_up[1], camera.view_up[2]);
    cam->SetClippingRange (camera.clip.val);
    cam->SetUseHorizontalViewAngle (0);
    cam->SetViewAngle (camera.fovy * 180.0 / M_PI);

    window_->SetSize (static_cast<int> (camera.window_size[0]), static_cast<int> (camera.window_size[1]));
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setCameraClipDistances (double near, double far)
{
    //rens_->InitTraversal ();

    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetClippingRange (near, far);
}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setCameraFieldOfView (double fovy)
{
    //rens_->InitTraversal ();

    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetUseHorizontalViewAngle (0);
    cam->SetViewAngle (fovy * 180.0 / M_PI);

}

/////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::resetCameraViewpoint (const std::string &id)
{
    vtkSmartPointer<vtkMatrix4x4> camera_pose;
    static CloudActorMap::iterator it = cloud_actor_map_->find (id);
    if (it != cloud_actor_map_->end ())
        camera_pose = it->second.viewpoint_transformation_;
    else
        return;

    // Prevent a segfault
    if (!camera_pose)
        return;

    // set all renderer to this viewpoint
    //rens_->InitTraversal ();


    vtkSmartPointer<vtkCamera> cam = renderer_->GetActiveCamera ();
    cam->SetPosition (camera_pose->GetElement (0, 3),
                      camera_pose->GetElement (1, 3),
                      camera_pose->GetElement (2, 3));

    cam->SetFocalPoint (camera_pose->GetElement (0, 3) - camera_pose->GetElement (0, 2),
                        camera_pose->GetElement (1, 3) - camera_pose->GetElement (1, 2),
                        camera_pose->GetElement (2, 3) - camera_pose->GetElement (2, 2));

    cam->SetViewUp (camera_pose->GetElement (0, 1),
                    camera_pose->GetElement (1, 1),
                    camera_pose->GetElement (2, 1));

    renderer_->SetActiveCamera (cam);
    renderer_->ResetCameraClippingRange ();
    renderer_->Render ();
}

////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, const std::string & id)
{
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
    {
        std::cout << "[addModelFromPolyData] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
        return (false);
    }

    vtkSmartPointer<vtkLODActor> actor;
    createActorFromVTKDataSet (polydata, actor);
    actor->GetProperty ()->SetRepresentationToWireframe ();
    renderer_->AddActor(actor);

    // Save the pointer/ID pair to the global actor map
    (*shape_actor_map_)[id] = actor;
    return (true);
}

////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata, vtkSmartPointer<vtkTransform> transform, const std::string & id)
{
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
    {
        std::cout << "[addModelFromPolyData] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl;
        return (false);
    }

    vtkSmartPointer <vtkTransformFilter> trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
    trans_filter->SetTransform (transform);
    trans_filter->SetInput (polydata);
    trans_filter->Update();

    // Create an Actor
    vtkSmartPointer <vtkLODActor> actor;
    createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
    actor->GetProperty ()->SetRepresentationToWireframe ();
    renderer_->AddActor(actor);

    // Save the pointer/ID pair to the global actor map
    (*shape_actor_map_)[id] = actor;
    return (true);
}


////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, const std::string &id)
{
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
        return std::cout << "[addModelFromPLYFile] A shape with id <"<<id<<"> already exists! Please choose a different id and retry.." << std::endl, false;

    vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New ();
    reader->SetFileName (filename.c_str ());

    vtkSmartPointer<vtkLODActor> actor;
    createActorFromVTKDataSet (reader->GetOutput (), actor);
    actor->GetProperty ()->SetRepresentationToWireframe ();
    renderer_->AddActor(actor);

    (*shape_actor_map_)[id] = actor;
    return true;
}

////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, vtkSmartPointer<vtkTransform> transform, const std::string &id)
{
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
        return std::cout << "[addModelFromPLYFile] A shape with id <"<<id<<"> already exists! Please choose a different id and retry." << std::endl, false;

    vtkSmartPointer <vtkPLYReader > reader = vtkSmartPointer<vtkPLYReader>::New ();
    reader->SetFileName (filename.c_str ());

    vtkSmartPointer <vtkTransformFilter> trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
    trans_filter->SetTransform (transform);
    trans_filter->SetInputConnection (reader->GetOutputPort ());

    vtkSmartPointer <vtkLODActor> actor;
    createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
    actor->GetProperty ()->SetRepresentationToWireframe ();
    renderer_->AddActor(actor);

    (*shape_actor_map_)[id] = actor;
    return (true);
}

bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& /*mesh*/, const std::string &/*id*/)
{
//     CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3);
// 
//     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
//     if (am_it != shape_actor_map_->end ())
//         return std::cout << "[addPolylineFromPolygonMesh] A shape with id <"<< id << "> already exists! Please choose a different id and retry.\n" << std::endl, false;
// 
//     vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
//     poly_points->SetNumberOfPoints (mesh.cloud.size().area());
// 
//     const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
//     for (int i = 0; i < mesh.cloud.cols; ++i)
//         poly_points->InsertPoint (i, cdata[i].x, cdata[i].y,cdata[i].z);
// 
// 
//     // Create a cell array to store the lines in and add the lines to it
//     vtkSmartPointer <vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
//     vtkSmartPointer <vtkPolyData> polyData;
//     allocVtkPolyData (polyData);
// 
//     for (size_t i = 0; i < mesh.polygons.size (); i++)
//     {
//         vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
//         polyLine->GetPointIds()->SetNumberOfIds(mesh.polygons[i].vertices.size());
//         for(unsigned int k = 0; k < mesh.polygons[i].vertices.size(); k++)
//         {
//             polyLine->GetPointIds()->SetId(k,mesh. polygons[i].vertices[k]);
//         }
// 
//         cells->InsertNextCell (polyLine);
//     }
// 
//     // Add the points to the dataset
//     polyData->SetPoints (poly_points);
// 
//     // Add the lines to the dataset
//     polyData->SetLines (cells);
// 
//     // Setup actor and mapper
//     vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
//     mapper->SetInput (polyData);
// 
//     vtkSmartPointer <vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
//     actor->SetMapper (mapper);
//     renderer_->AddActor(actor);
// 
//     // Save the pointer/ID pair to the global actor map
//     (*shape_actor_map_)[id] = actor;
    return (true);
}


///////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setRepresentationToSurfaceForAllActors ()
{
    vtkActorCollection * actors = renderer_->GetActors ();
    actors->InitTraversal ();
    vtkActor * actor;
    while ((actor = actors->GetNextActor ()) != NULL)
        actor->GetProperty ()->SetRepresentationToSurface ();
}

///////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setRepresentationToPointsForAllActors ()
{
    vtkActorCollection * actors = renderer_->GetActors ();
    actors->InitTraversal ();
    vtkActor * actor;
    while ((actor = actors->GetNextActor ()) != NULL)
        actor->GetProperty ()->SetRepresentationToPoints ();
}

///////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::setRepresentationToWireframeForAllActors ()
{
    vtkActorCollection * actors = renderer_->GetActors ();
    actors->InitTraversal ();
    vtkActor *actor;
    while ((actor = actors->GetNextActor ()) != NULL)
        actor->GetProperty ()->SetRepresentationToWireframe ();
}

//////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::updateCells (vtkSmartPointer<vtkIdTypeArray> &cells, vtkSmartPointer<vtkIdTypeArray> &initcells, vtkIdType nr_points)
{
    // If no init cells and cells has not been initialized...
    if (!cells)
        cells = vtkSmartPointer<vtkIdTypeArray>::New ();

    // If we have less values then we need to recreate the array
    if (cells->GetNumberOfTuples () < nr_points)
    {
        cells = vtkSmartPointer<vtkIdTypeArray>::New ();

        // If init cells is given, and there's enough data in it, use it
        if (initcells && initcells->GetNumberOfTuples () >= nr_points)
        {
            cells->DeepCopy (initcells);
            cells->SetNumberOfComponents (2);
            cells->SetNumberOfTuples (nr_points);
        }
        else
        {
            // If the number of tuples is still too small, we need to recreate the array
            cells->SetNumberOfComponents (2);
            cells->SetNumberOfTuples (nr_points);
            vtkIdType *cell = cells->GetPointer (0);
            // Fill it with 1s
            std::fill_n (cell, nr_points * 2, 1);
            cell++;
            for (vtkIdType i = 0; i < nr_points; ++i, cell += 2)
                *cell = i;
            // Save the results in initcells
            initcells = vtkSmartPointer<vtkIdTypeArray>::New ();
            initcells->DeepCopy (cells);
        }
    }
    else
    {
        // The assumption here is that the current set of cells has more data than needed
        cells->SetNumberOfComponents (2);
        cells->SetNumberOfTuples (nr_points);
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata)
{polydata = vtkSmartPointer<vtkAppendPolyData>::New (); }

void cv::viz::Viz3d::VizImpl::allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata)
{ polydata = vtkSmartPointer<vtkPolyData>::New (); }

void cv::viz::Viz3d::VizImpl::allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata)
{ polydata = vtkSmartPointer<vtkUnstructuredGrid>::New (); }

//////////////////////////////////////////////////////////////////////////////////////////////
void cv::viz::convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion<float> &orientation, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix)
{
    // set rotation
    Eigen::Matrix3f rot = orientation.toRotationMatrix ();
    for (int i = 0; i < 3; i++)
        for (int k = 0; k < 3; k++)
            vtk_matrix->SetElement (i, k, rot (i, k));

    // set translation
    vtk_matrix->SetElement (0, 3, origin (0));
    vtk_matrix->SetElement (1, 3, origin (1));
    vtk_matrix->SetElement (2, 3, origin (2));
    vtk_matrix->SetElement (3, 3, 1.0f);
}


//////////////////////////////////////////////////////////////////////////////////////////////

void cv::viz::Viz3d::VizImpl::setFullScreen (bool mode)
{
    if (window_)
        window_->SetFullScreen (mode);
}

void cv::viz::Viz3d::VizImpl::setWindowName (const std::string &name)
{
    if (window_)
        window_->SetWindowName (name.c_str ());
}

void cv::viz::Viz3d::VizImpl::setWindowPosition (int x, int y) { window_->SetPosition (x, y); }
void cv::viz::Viz3d::VizImpl::setWindowSize (int xw, int yw) { window_->SetSize (xw, yw); }

bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& /*mesh*/, const Mat& /*mask*/, const std::string &/*id*/)
{
//     CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
//     CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
//     CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
// 
//     if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
//         return std::cout << "[addPolygonMesh] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;
// 
//     //    int rgb_idx = -1;
//     //    std::vector<sensor_msgs::PointField> fields;
// 
// 
//     //    rgb_idx = temp_viz::getFieldIndex (*cloud, "rgb", fields);
//     //    if (rgb_idx == -1)
//     //      rgb_idx = temp_viz::getFieldIndex (*cloud, "rgba", fields);
// 
//     vtkSmartPointer<vtkUnsignedCharArray> colors_array;
// #if 1
//     if (!mesh.colors.empty())
//     {
//         colors_array = vtkSmartPointer<vtkUnsignedCharArray>::New ();
//         colors_array->SetNumberOfComponents (3);
//         colors_array->SetName ("Colors");
// 
//         const unsigned char* data = mesh.colors.ptr<unsigned char>();
// 
//         //TODO check mask
//         CV_Assert(mask.empty()); //because not implemented;
// 
//         for(int i = 0; i < mesh.colors.cols; ++i)
//             colors_array->InsertNextTupleValue(&data[i*3]);
// 
//         //      temp_viz::RGB rgb_data;
//         //      for (size_t i = 0; i < cloud->size (); ++i)
//         //      {
//         //        if (!isFinite (cloud->points[i]))
//         //          continue;
//         //        memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (temp_viz::RGB));
//         //        unsigned char color[3];
//         //        color[0] = rgb_data.r;
//         //        color[1] = rgb_data.g;
//         //        color[2] = rgb_data.b;
//         //        colors->InsertNextTupleValue (color);
//         //      }
//     }
// #endif
// 
//     // Create points from polyMesh.cloud
//     vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
//     vtkIdType nr_points = mesh.cloud.size().area();
// 
//     points->SetNumberOfPoints (nr_points);
// 
// 
//     // Get a pointer to the beginning of the data array
//     float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
// 
// 
//     std::vector<int> lookup;
//     // If the dataset is dense (no NaNs)
//     if (mask.empty())
//     {
//         cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
//         mesh.cloud.copyTo(hdr);
//     }
//     else
//     {
//         lookup.resize (nr_points);
// 
//         const unsigned char *mdata = mask.ptr<unsigned char>();
//         const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
//         cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
// 
//         int j = 0;    // true point index
//         for (int i = 0; i < nr_points; ++i)
//             if(mdata[i])
//             {
//                 lookup[i] = j;
//                 out[j++] = cdata[i];
//             }
//         nr_points = j;
//         points->SetNumberOfPoints (nr_points);
//     }
// 
//     // Get the maximum size of a polygon
//     int max_size_of_polygon = -1;
//     for (size_t i = 0; i < mesh.polygons.size (); ++i)
//         if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
//             max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
// 
//     vtkSmartPointer<vtkLODActor> actor;
// 
//     if (mesh.polygons.size () > 1)
//     {
//         // Create polys from polyMesh.polygons
//         vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
//         vtkIdType *cell = cell_array->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
//         int idx = 0;
//         if (lookup.size () > 0)
//         {
//             for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
//             {
//                 size_t n_points = mesh.polygons[i].vertices.size ();
//                 *cell++ = n_points;
//                 //cell_array->InsertNextCell (n_points);
//                 for (size_t j = 0; j < n_points; j++, ++idx)
//                     *cell++ = lookup[mesh.polygons[i].vertices[j]];
//                 //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
//             }
//         }
//         else
//         {
//             for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
//             {
//                 size_t n_points = mesh.polygons[i].vertices.size ();
//                 *cell++ = n_points;
//                 //cell_array->InsertNextCell (n_points);
//                 for (size_t j = 0; j < n_points; j++, ++idx)
//                     *cell++ = mesh.polygons[i].vertices[j];
//                 //cell_array->InsertCellPoint (vertices[i].vertices[j]);
//             }
//         }
//         vtkSmartPointer<vtkPolyData> polydata;
//         allocVtkPolyData (polydata);
//         cell_array->GetData ()->SetNumberOfValues (idx);
//         cell_array->Squeeze ();
//         polydata->SetStrips (cell_array);
//         polydata->SetPoints (points);
// 
//         if (colors_array)
//             polydata->GetPointData ()->SetScalars (colors_array);
// 
//         createActorFromVTKDataSet (polydata, actor, false);
//     }
//     else
//     {
//         vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
//         size_t n_points = mesh.polygons[0].vertices.size ();
//         polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
// 
//         if (lookup.size () > 0)
//         {
//             for (size_t j = 0; j < n_points - 1; ++j)
//                 polygon->GetPointIds ()->SetId (j, lookup[mesh.polygons[0].vertices[j]]);
//         }
//         else
//         {
//             for (size_t j = 0; j < n_points - 1; ++j)
//                 polygon->GetPointIds ()->SetId (j, mesh.polygons[0].vertices[j]);
//         }
//         vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
//         allocVtkUnstructuredGrid (poly_grid);
//         poly_grid->Allocate (1, 1);
//         poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
//         poly_grid->SetPoints (points);
//         poly_grid->Update ();
//         if (colors_array)
//             poly_grid->GetPointData ()->SetScalars (colors_array);
// 
//         createActorFromVTKDataSet (poly_grid, actor, false);
//     }
//     renderer_->AddActor (actor);
//     actor->GetProperty ()->SetRepresentationToSurface ();
//     // Backface culling renders the visualization slower, but guarantees that we see all triangles
//     actor->GetProperty ()->BackfaceCullingOff ();
//     actor->GetProperty ()->SetInterpolationToFlat ();
//     actor->GetProperty ()->EdgeVisibilityOff ();
//     actor->GetProperty ()->ShadingOff ();
// 
//     // Save the pointer/ID pair to the global actor map
//     (*cloud_actor_map_)[id].actor = actor;
//     //if (vertices.size () > 1)
//     //  (*cloud_actor_map_)[id].cells = static_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData ();
// 
//     const Eigen::Vector4f& sensor_origin = Eigen::Vector4f::Zero ();
//     const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternionf::Identity ();
// 
//     // Save the viewpoint transformation matrix to the global actor map
//     vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
//     convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
//     (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
// 
//     return (true);
return true;
}


bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& /*mesh*/, const cv::Mat& /*mask*/, const std::string &/*id*/)
{
//     CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ());
//     CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3));
//     CV_Assert(mask.empty() || (!mask.empty() && mask.size() == mesh.cloud.size() && mask.type() == CV_8U));
// 
//     // Check to see if this ID entry already exists (has it been already added to the visualizer?)
//     CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
//     if (am_it == cloud_actor_map_->end ())
//         return (false);
// 
//     // Get the current poly data
//     vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
//     if (!polydata)
//         return (false);
//     vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
//     if (!cells)
//         return (false);
//     vtkSmartPointer<vtkPoints> points   = polydata->GetPoints ();
//     // Copy the new point array in
//     vtkIdType nr_points = mesh.cloud.size().area();
//     points->SetNumberOfPoints (nr_points);
// 
//     // Get a pointer to the beginning of the data array
//     float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
// 
//     int ptr = 0;
//     std::vector<int> lookup;
//     // If the dataset is dense (no NaNs)
//     if (mask.empty())
//     {
//         cv::Mat hdr(mesh.cloud.size(), CV_32FC3, (void*)data);
//         mesh.cloud.copyTo(hdr);
// 
//     }
//     else
//     {
//         lookup.resize (nr_points);
// 
//         const unsigned char *mdata = mask.ptr<unsigned char>();
//         const cv::Point3f *cdata = mesh.cloud.ptr<cv::Point3f>();
//         cv::Point3f* out = reinterpret_cast<cv::Point3f*>(data);
// 
//         int j = 0;    // true point index
//         for (int i = 0; i < nr_points; ++i)
//             if(mdata[i])
//             {
//                 lookup[i] = j;
//                 out[j++] = cdata[i];
//             }
//         nr_points = j;
//         points->SetNumberOfPoints (nr_points);;
//     }
// 
//     // Update colors
//     vtkUnsignedCharArray* colors_array = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
// 
//     if (!mesh.colors.empty() && colors_array)
//     {
//         if (mask.empty())
//         {
//             const unsigned char* data = mesh.colors.ptr<unsigned char>();
//             for(int i = 0; i < mesh.colors.cols; ++i)
//                 colors_array->InsertNextTupleValue(&data[i*3]);
//         }
//         else
//         {
//             const unsigned char* color = mesh.colors.ptr<unsigned char>();
//             const unsigned char* mdata = mask.ptr<unsigned char>();
// 
//             int j = 0;
//             for(int i = 0; i < mesh.colors.cols; ++i)
//                 if (mdata[i])
//                     colors_array->SetTupleValue (j++, &color[i*3]);
// 
//         }
//     }
// 
//     // Get the maximum size of a polygon
//     int max_size_of_polygon = -1;
//     for (size_t i = 0; i < mesh.polygons.size (); ++i)
//         if (max_size_of_polygon < static_cast<int> (mesh.polygons[i].vertices.size ()))
//             max_size_of_polygon = static_cast<int> (mesh.polygons[i].vertices.size ());
// 
//     // Update the cells
//     cells = vtkSmartPointer<vtkCellArray>::New ();
//     vtkIdType *cell = cells->WritePointer (mesh.polygons.size (), mesh.polygons.size () * (max_size_of_polygon + 1));
//     int idx = 0;
//     if (lookup.size () > 0)
//     {
//         for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
//         {
//             size_t n_points = mesh.polygons[i].vertices.size ();
//             *cell++ = n_points;
//             for (size_t j = 0; j < n_points; j++, cell++, ++idx)
//                 *cell = lookup[mesh.polygons[i].vertices[j]];
//         }
//     }
//     else
//     {
//         for (size_t i = 0; i < mesh.polygons.size (); ++i, ++idx)
//         {
//             size_t n_points = mesh.polygons[i].vertices.size ();
//             *cell++ = n_points;
//             for (size_t j = 0; j < n_points; j++, cell++, ++idx)
//                 *cell = mesh.polygons[i].vertices[j];
//         }
//     }
//     cells->GetData ()->SetNumberOfValues (idx);
//     cells->Squeeze ();
//     // Set the the vertices
//     polydata->SetStrips (cells);
//     polydata->Update ();
    return (true);
}

////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color, bool display_length, const std::string &id)
{
    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
        return std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl, false;

    // Create an Actor
    vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
    leader->GetPositionCoordinate()->SetCoordinateSystemToWorld ();
    leader->GetPositionCoordinate()->SetValue (p1.x, p1.y, p1.z);
    leader->GetPosition2Coordinate()->SetCoordinateSystemToWorld ();
    leader->GetPosition2Coordinate()->SetValue (p2.x, p2.y, p2.z);
    leader->SetArrowStyleToFilled();
    leader->SetArrowPlacementToPoint2 ();

    if (display_length)
        leader->AutoLabelOn ();
    else
        leader->AutoLabelOff ();

    Color c = vtkcolor(color);
    leader->GetProperty ()->SetColor (c.val);
    renderer_->AddActor (leader);

    // Save the pointer/ID pair to the global actor map
    (*shape_actor_map_)[id] = leader;
    return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////
bool cv::viz::Viz3d::VizImpl::addArrow (const cv::Point3f &p1, const cv::Point3f &p2, const Color& color_line, const Color& color_text, const std::string &id)
{
    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
    {
        std::cout << "[addArrow] A shape with id <" << id << "> already exists! Please choose a different id and retry." << std::endl;
        return (false);
    }

    // Create an Actor
    vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
    leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
    leader->GetPositionCoordinate ()->SetValue (p1.x, p1.y, p1.z);
    leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
    leader->GetPosition2Coordinate ()->SetValue (p2.x, p2.y, p2.z);
    leader->SetArrowStyleToFilled ();
    leader->AutoLabelOn ();

    Color ct = vtkcolor(color_text);
    leader->GetLabelTextProperty()->SetColor(ct.val);

    Color cl = vtkcolor(color_line);
    leader->GetProperty ()->SetColor (cl.val);
    renderer_->AddActor (leader);

    // Save the pointer/ID pair to the global actor map
    (*shape_actor_map_)[id] = leader;
    return (true);
}

bool cv::viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& color, const std::string &id)
{
    CV_Assert(cloud.type() == CV_32FC3 && cloud.rows == 1);

    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
    vtkSmartPointer<vtkPolygon> polygon    = vtkSmartPointer<vtkPolygon>::New ();

    int total = cloud.size().area();
    points->SetNumberOfPoints (total);
    polygon->GetPointIds ()->SetNumberOfIds (total);

    for (int i = 0; i < total; ++i)
    {
        cv::Point3f p = cloud.ptr<cv::Point3f>()[i];
        points->SetPoint (i, p.x, p.y, p.z);
        polygon->GetPointIds ()->SetId (i, i);
    }

    vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
    allocVtkUnstructuredGrid (poly_grid);
    poly_grid->Allocate (1, 1);
    poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
    poly_grid->SetPoints (points);
    poly_grid->Update ();


    //////////////////////////////////////////////////////
    vtkSmartPointer<vtkDataSet> data = poly_grid;

    Color c = vtkcolor(color);

    // Check to see if this ID entry already exists (has it been already added to the visualizer?)
    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
    if (am_it != shape_actor_map_->end ())
    {
        vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();

        // Add old data
        all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());

        // Add new data
        vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
        surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
        vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
        all_data->AddInput (poly_data);

        // Create an Actor
        vtkSmartPointer<vtkLODActor> actor;
        createActorFromVTKDataSet (all_data->GetOutput (), actor);
        actor->GetProperty ()->SetRepresentationToWireframe ();
        actor->GetProperty ()->SetColor (c.val);
        actor->GetMapper ()->ScalarVisibilityOff ();
        actor->GetProperty ()->BackfaceCullingOff ();

        removeActorFromRenderer (am_it->second);
        renderer_->AddActor (actor);

        // Save the pointer/ID pair to the global actor map
        (*shape_actor_map_)[id] = actor;
    }
    else
    {
        // Create an Actor
        vtkSmartPointer<vtkLODActor> actor;
        createActorFromVTKDataSet (data, actor);
        actor->GetProperty ()->SetRepresentationToWireframe ();
        actor->GetProperty ()->SetColor (c.val);
        actor->GetMapper ()->ScalarVisibilityOff ();
        actor->GetProperty ()->BackfaceCullingOff ();
        renderer_->AddActor (actor);

        // Save the pointer/ID pair to the global actor map
        (*shape_actor_map_)[id] = actor;
    }

    return (true);
}

void cv::viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget, const Affine3f &pose)
{
    WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
    bool exists = wam_itr != widget_actor_map_->end();
    if (exists)
    {
        // Remove it if it exists and add it again
        removeActorFromRenderer(wam_itr->second.actor);
    }
    // Get the actor and set the user matrix
    vtkProp3D *actor = vtkProp3D::SafeDownCast(WidgetAccessor::getProp(widget));
    if (actor)
    {
        // If the actor is 3D, apply pose
        vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
        actor->SetUserMatrix (matrix);
        actor->Modified();
    }
    // If the actor is a vtkFollower, then it should always face the camera
    vtkFollower *follower = vtkFollower::SafeDownCast(actor);
    if (follower)
    {
        follower->SetCamera(renderer_->GetActiveCamera());
    }

    renderer_->AddActor(WidgetAccessor::getProp(widget));
    (*widget_actor_map_)[id].actor = WidgetAccessor::getProp(widget);
}

void cv::viz::Viz3d::VizImpl::removeWidget(const String &id)
{
    WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
    bool exists = wam_itr != widget_actor_map_->end();
    CV_Assert(exists);
    CV_Assert(removeActorFromRenderer (wam_itr->second.actor));
    widget_actor_map_->erase(wam_itr);
}

cv::viz::Widget cv::viz::Viz3d::VizImpl::getWidget(const String &id) const
{
    WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
    bool exists = wam_itr != widget_actor_map_->end();
    CV_Assert(exists);

    Widget widget;
    WidgetAccessor::setProp(widget, wam_itr->second.actor);
    return widget;
}

void cv::viz::Viz3d::VizImpl::setWidgetPose(const String &id, const Affine3f &pose)
{
    WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
    bool exists = wam_itr != widget_actor_map_->end();
    CV_Assert(exists);

    vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
    CV_Assert(actor);

    vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
    actor->SetUserMatrix (matrix);
    actor->Modified ();
}

void cv::viz::Viz3d::VizImpl::updateWidgetPose(const String &id, const Affine3f &pose)
{
    WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id);
    bool exists = wam_itr != widget_actor_map_->end();
    CV_Assert(exists);

    vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
    CV_Assert(actor);

    vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
    if (!matrix)
    {
        setWidgetPose(id, pose);
        return ;
    }
    Matx44f matrix_cv = convertToMatx(matrix);
    Affine3f updated_pose = pose * Affine3f(matrix_cv);
    matrix = convertToVtkMatrix(updated_pose.matrix);

    actor->SetUserMatrix (matrix);
    actor->Modified ();
}

cv::Affine3f cv::viz::Viz3d::VizImpl::getWidgetPose(const String &id) const
{
    WidgetActorMap::const_iterator wam_itr = widget_actor_map_->find(id);
    bool exists = wam_itr != widget_actor_map_->end();
    CV_Assert(exists);

    vtkProp3D *actor = vtkProp3D::SafeDownCast(wam_itr->second.actor);
    CV_Assert(actor);

    vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
    Matx44f matrix_cv = convertToMatx(matrix);
    return Affine3f(matrix_cv);
}