manual_registration.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 * 
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: $
 *
 * @author: Koen Buys - KU Leuven
 */

#include <pcl/apps/manual_registration.h>

//QT4
#include <QApplication>
#include <QMutexLocker>
#include <QEvent>
#include <QObject>

// VTK
#include <vtkRenderWindow.h>
#include <vtkRendererCollection.h>
#include <vtkCamera.h>

using namespace pcl;
using namespace std;

ManualRegistration::ManualRegistration ()
{
  //Create a timer
  vis_timer_ = new QTimer (this);
  vis_timer_->start (5);//5ms

  connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot()));

  ui_ = new Ui::MainWindow;
  ui_->setupUi (this);
  
  this->setWindowTitle ("PCL Manual Registration");

  // Set up the source window
  vis_src_.reset (new pcl::visualization::PCLVisualizer ("", false));
  ui_->qvtk_widget_src->SetRenderWindow (vis_src_->getRenderWindow ());
  vis_src_->setupInteractor (ui_->qvtk_widget_src->GetInteractor (), ui_->qvtk_widget_src->GetRenderWindow ());
  vis_src_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
  ui_->qvtk_widget_src->update ();

  vis_src_->registerPointPickingCallback (&ManualRegistration::SourcePointPickCallback, *this);

  // Set up the destination window
  vis_dst_.reset (new pcl::visualization::PCLVisualizer ("", false));
  ui_->qvtk_widget_dst->SetRenderWindow (vis_dst_->getRenderWindow ());
  vis_dst_->setupInteractor (ui_->qvtk_widget_dst->GetInteractor (), ui_->qvtk_widget_dst->GetRenderWindow ());
  vis_dst_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT);
  ui_->qvtk_widget_dst->update ();

  vis_dst_->registerPointPickingCallback (&ManualRegistration::DstPointPickCallback, *this);


  // Connect all buttons
  connect (ui_->confirmSrcPointButton, SIGNAL(clicked()), this, SLOT(confirmSrcPointPressed()));
  connect (ui_->confirmDstPointButton, SIGNAL(clicked()), this, SLOT(confirmDstPointPressed()));
  connect (ui_->calculateButton, SIGNAL(clicked()), this, SLOT(calculatePressed()));
  connect (ui_->clearButton, SIGNAL(clicked()), this, SLOT(clearPressed()));
  connect (ui_->orthoButton, SIGNAL(stateChanged(int)), this, SLOT(orthoChanged(int)));
  connect (ui_->applyTransformButton, SIGNAL(clicked()), this, SLOT(applyTransformPressed()));
  connect (ui_->refineButton, SIGNAL(clicked()), this, SLOT(refinePressed()));
  connect (ui_->undoButton, SIGNAL(clicked()), this, SLOT(undoPressed()));
  connect (ui_->safeButton, SIGNAL(clicked()), this, SLOT(safePressed()));

  cloud_src_modified_ = true; // first iteration is always a new pointcloud
  cloud_dst_modified_ = true;
}

void
ManualRegistration::SourcePointPickCallback (const pcl::visualization::PointPickingEvent& event, void*)
{
  // Check to see if we got a valid point. Early exit.
  int idx = event.getPointIndex ();
  if (idx == -1)
    return;

  // Get the point that was picked
  event.getPoint (src_point_.x, src_point_.y, src_point_.z);
  PCL_INFO ("Src Window: Clicked point %d with X:%f Y:%f Z:%f\n", idx, src_point_.x, src_point_.y, src_point_.z);
  src_point_selected_ = true;
}

void
ManualRegistration::DstPointPickCallback (const pcl::visualization::PointPickingEvent& event, void*)
{
  // Check to see if we got a valid point. Early exit.
  int idx = event.getPointIndex ();
  if (idx == -1)
    return;

  // Get the point that was picked
  event.getPoint (dst_point_.x, dst_point_.y, dst_point_.z);
  PCL_INFO ("Dst Window: Clicked point %d with X:%f Y:%f Z:%f\n", idx, dst_point_.x, dst_point_.y, dst_point_.z);
  dst_point_selected_ = true;
}

void 
ManualRegistration::confirmSrcPointPressed()
{
  if(src_point_selected_)
  {
    src_pc_.points.push_back(src_point_);
    PCL_INFO ("Selected %d source points\n", src_pc_.points.size());
    src_point_selected_ = false;
    src_pc_.width = src_pc_.points.size();
  }
  else
  {
    PCL_INFO ("Please select a point in the source window first\n");
  }
}

void 
ManualRegistration::confirmDstPointPressed()
{
  if(dst_point_selected_)
  {
    dst_pc_.points.push_back(dst_point_);
    PCL_INFO ("Selected %d destination points\n", dst_pc_.points.size());
    dst_point_selected_ = false;
    dst_pc_.width = dst_pc_.points.size();
  }
  else
  {
    PCL_INFO ("Please select a point in the destination window first\n");
  }
}

void 
ManualRegistration::calculatePressed()
{
  if(dst_pc_.points.size() != src_pc_.points.size())
  {
    PCL_INFO ("You haven't selected an equal amount of points, please do so\n");
    return;
  }
  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> tfe;
  tfe.estimateRigidTransformation(src_pc_, dst_pc_, transform_);
  std::cout << "Transform : " << std::endl << transform_ << std::endl;
}

void
ManualRegistration::clearPressed()
{
  dst_point_selected_ = false;
  src_point_selected_ = false;
  src_pc_.points.clear();
  dst_pc_.points.clear();
  src_pc_.height = 1; src_pc_.width = 0;
  dst_pc_.height = 1; dst_pc_.width = 0;
}

void 
ManualRegistration::orthoChanged (int state)
{
  PCL_INFO ("Ortho state %d\n", state);
  if(state == 0) // Not selected
  {
    vis_src_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(0);
    vis_dst_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(0);
  }
  if(state == 2) // Selected
  {
    vis_src_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(1);
    vis_dst_->getRenderWindow ()->GetRenderers()->GetFirstRenderer()->GetActiveCamera()->SetParallelProjection(1);
  }
  ui_->qvtk_widget_src->update();
  ui_->qvtk_widget_dst->update();
}

//TODO
void 
ManualRegistration::applyTransformPressed()
{
}

void
ManualRegistration::refinePressed()
{
}

void
ManualRegistration::undoPressed()
{
}

void
ManualRegistration::safePressed()
{
}

void 
ManualRegistration::timeoutSlot ()
{
  if(cloud_src_present_ && cloud_src_modified_)
  {
    if(!vis_src_->updatePointCloud(cloud_src_, "cloud_src_"))
    {
      vis_src_->addPointCloud (cloud_src_, "cloud_src_");
      vis_src_->resetCameraViewpoint("cloud_src_");
    }
    cloud_src_modified_ = false;
  }
  if(cloud_dst_present_ && cloud_dst_modified_)
  {
    if(!vis_dst_->updatePointCloud(cloud_dst_, "cloud_dst_"))
    {
      vis_dst_->addPointCloud (cloud_dst_, "cloud_dst_");
      vis_dst_->resetCameraViewpoint("cloud_dst_");
    }
    cloud_dst_modified_ = false;
  }
  ui_->qvtk_widget_src->update();
  ui_->qvtk_widget_dst->update();
}

void
print_usage ()
{
  PCL_INFO ("manual_registration cloud1.pcd cloud2.pcd\n");
  PCL_INFO ("\t cloud1 \t source cloud\n");
  PCL_INFO ("\t cloud2 \t destination cloud\n");
}

int
main (int argc, char** argv)
{
  QApplication app(argc, argv);

  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_src (new pcl::PointCloud<pcl::PointXYZRGBA>);
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_dst (new pcl::PointCloud<pcl::PointXYZRGBA>);

  if(argc < 3)
  {
    PCL_ERROR ("Incorrect usage\n");
    print_usage();
  }

  // TODO do this with PCL console
  if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_src) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file %s \n", argv[1]);
    return (-1);
  }
  if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_dst) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file %s \n", argv[2]);
    return (-1);
  }

  ManualRegistration man_reg;

  man_reg.setSrcCloud(cloud_src);
  man_reg.setDstCloud(cloud_dst);

  man_reg.show();

  return (app.exec());
}