bool
pcl::EnsensoGrabber::openDevice (const int device)
{
if (device_open_)
PCL_THROW_EXCEPTION (pcl::IOException, "Cannot open multiple devices!");
PCL_INFO ("Opening Ensenso stereo camera id = %d\n", device);
try
{
// Create a pointer referencing the camera's tree item, for easier access:
camera_ = (*root_)[itmCameras][itmBySerialNo][device];
if (!camera_.exists () || camera_[itmType] != valStereo)
{
PCL_THROW_EXCEPTION (pcl::IOException, "Please connect a single stereo camera to your computer!");
}
NxLibCommand open (cmdOpen);
open.parameters ()[itmCameras] = camera_[itmSerialNumber].asString ();
open.execute ();
}
catch (NxLibException &ex)
{
ensensoExceptionHandling (ex, "openDevice");
return (false);
}
device_open_ = true;
return (true);
}
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initData ()
{
if (border_policy_ != BORDER_POLICY_IGNORE &&
border_policy_ != BORDER_POLICY_MIRROR)
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
if (normal_estimation_method_ != COVARIANCE_MATRIX &&
normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
normal_estimation_method_ != SIMPLE_3D_GRADIENT)
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
// compute derivatives
if (diff_x_ != NULL) delete[] diff_x_;
if (diff_y_ != NULL) delete[] diff_y_;
if (depth_data_ != NULL) delete[] depth_data_;
if (distance_map_ != NULL) delete[] distance_map_;
diff_x_ = NULL;
diff_y_ = NULL;
depth_data_ = NULL;
distance_map_ = NULL;
if (normal_estimation_method_ == COVARIANCE_MATRIX)
initCovarianceMatrixMethod ();
else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
initAverage3DGradientMethod ();
else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
initAverageDepthChangeMethod ();
else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
initSimple3DGradientMethod ();
}
template <typename PointT> void
pcl::common::deleteCols (const PointCloud<PointT>& input, PointCloud<PointT>& output,
const size_t& amount)
{
if (amount <= 0 || amount > (input.width/2))
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::deleteCols] error: amount must be in ]0.."
<< (input.width/2) << "] !");
if (!input.isOrganized ())
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::deleteCols] error: "
<< "columns delete requires organised point cloud");
uint32_t old_height = input.height;
uint32_t old_width = input.width;
uint32_t new_width = old_width - 2 * amount;
for(size_t j = 0; j < old_height; j++)
{
typename PointCloud<PointT>::iterator start = output.begin () + j * new_width;
output.erase (start, start + amount);
start = output.begin () + (j+1) * new_width;
output.erase (start, start + amount);
}
output.height = old_height;
output.width = new_width;
}
template <typename PointT> void
pcl::common::duplicateColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
const size_t& amount)
{
if (amount <= 0)
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::duplicateColumns] error: amount must be ]0.."
<< (input.width/2) << "] !");
if (!input.isOrganized () || amount > (input.width/2))
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::duplicateColumns] error: "
<< "columns expansion requires organised point cloud");
size_t old_height = input.height;
size_t old_width = input.width;
size_t new_width = old_width + 2*amount;
if (&input != &output)
output = input;
output.reserve (new_width * old_height);
for (size_t j = 0; j < old_height; ++j)
for(size_t i = 0; i < amount; ++i)
{
typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
output.insert (start, *start);
start = output.begin () + (j * new_width) + old_width + i;
output.insert (start, *start);
}
output.width = new_width;
output.height = old_height;
}
template<typename Container, typename PointT> void
octree_base<Container, PointT>::loadFromFile ()
{
// Open JSON
std::vector<char> idx_input;
boost::uintmax_t len = boost::filesystem::file_size (treepath_);
idx_input.resize (len + 1);
std::ifstream f (treepath_.string ().c_str (), std::ios::in);
f.read (&(idx_input.front ()), len);
idx_input.back () = '\0';
// Parse JSON
boost::shared_ptr<cJSON> idx (cJSON_Parse (&(idx_input.front ())), cJSON_Delete);
cJSON* name = cJSON_GetObjectItem (idx.get (), "name");
cJSON* version = cJSON_GetObjectItem (idx.get (), "version");
cJSON* pointtype = cJSON_GetObjectItem (idx.get (), "pointtype");
cJSON* lod = cJSON_GetObjectItem (idx.get (), "lod");
cJSON* numpts = cJSON_GetObjectItem (idx.get (), "numpts");
cJSON* coord = cJSON_GetObjectItem (idx.get (), "coord_system");
// Validate JSON
if (!((name) && (version) && (pointtype) && (lod) && (numpts) && (coord)))
{
PCL_ERROR ( "index %s failed to parse!\n", treepath_.c_str ());
PCL_THROW_EXCEPTION (PCLException, "Outofcore Octree Parse Failure\n");
}
if ((name->type != cJSON_String) || (version->type != cJSON_Number) || (pointtype->type != cJSON_String)
|| (lod->type != cJSON_Number) || (numpts->type != cJSON_Array) || (coord->type != cJSON_String))
{
PCL_ERROR ( "index failed to parse!\n",treepath_.c_str ());
PCL_THROW_EXCEPTION (PCLException, "Outofcore Octree Exception: Index failed to parse\n");
}
if (version->valuedouble != 2.0 && version->valuedouble != 3.0)// || version->valuedouble != 3.0)
{
PCL_ERROR ( "index failed to parse!\n",treepath_.c_str ());
PCL_THROW_EXCEPTION (PCLException, "Outofcore Octree Parse Failure: Incompatible Version of Outofcore Octree\n");
}
if ((lod->valueint + 1) != cJSON_GetArraySize (numpts))
{
PCL_ERROR ( "[pcl::outofcore::octree_base] Index failed to parse: %s\n",treepath_.c_str ());
PCL_THROW_EXCEPTION (PCLException, "Outofcore Octree Prase Failure: LOD and array size of points is not valid\n");
}
// Get Data
lodPoints_.resize (lod->valueint + 1);
for (int i = 0; i < (lod->valueint + 1); i++)
{
//cJSON doesn't have explicit 64bit int, have to use double, get up to 2^52
lodPoints_[i] = static_cast<boost::uint64_t> (cJSON_GetArrayItem (numpts, i)->valuedouble );
}
max_depth_ = lod->valueint;
coord_system_ = coord->valuestring;
}
octree_base<Container, PointT>::octree_base (const int max_depth, const double min[3], const double max[3], const boost::filesystem::path& rootname, const std::string& coord_sys)
: root_ ()
, read_write_mutex_ ()
, lodPoints_ ()
, max_depth_ ()
, treepath_ ()
, coord_system_ ()
, resolution_ ()
{
// Check file extension
if (boost::filesystem::extension (rootname) != octree_base_node<Container, PointT>::node_index_extension)
{
PCL_ERROR ( "[pcl::outofcore::octree_base] the tree must be created with a root_ node ending in .oct_idx\n" );
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::octree_base] Bad extension. Tree must be created with node ending in .oct_idx\n");
}
coord_system_ = coord_sys;
// Get fullpath and recreate directories
boost::filesystem::path dir = rootname.parent_path ();
if (!boost::filesystem::exists (dir))
{
boost::filesystem::create_directory (dir);
}
// todo: Why is overwriting an existing tree not permitted when setting the
// max depth, but supported when setting node dims?
for (int i = 0; i < 8; i++)
{
boost::filesystem::path childdir = dir / boost::lexical_cast<std::string> (i);
if (boost::filesystem::exists (childdir))
{
PCL_ERROR ("[pcl::outofcore::octree_base] A dir named %d exists under the root node. Overwriting an existing tree is not supported.\n", i);
PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::octree_base] Directory exists; Overwriting an existing tree is not supported\n");
}
}
// Create root node
root_ = new octree_base_node<Container, PointT> (max_depth, min, max, this, rootname);
root_->saveIdx (false);
// max_depth_ is set when creating the root_ node
lodPoints_.resize (max_depth_ + 1);
// Set root nodes file path
treepath_ = dir / (boost::filesystem::basename (rootname) + TREE_EXTENSION_);
saveToFile ();
}
octree_base<Container, PointT>::octree_base (const boost::filesystem::path& rootname, const bool load_all)
: root_ ()
, read_write_mutex_ ()
, lodPoints_ ()
, max_depth_ ()
, treepath_ ()
, coord_system_ ()
, resolution_ ()
{
// Check file extension
if (boost::filesystem::extension (rootname) != octree_base_node<Container, PointT>::node_index_extension)
{
PCL_ERROR ( "[pcl::outofcore::octree_base] Wrong root node file extension: %s. The tree must have a root node ending in %s\n", boost::filesystem::extension (rootname).c_str (), octree_base_node<Container, PointT>::node_index_extension.c_str () );
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::octree_base] Bad extension. Tree must have a root node ending in .oct_idx\n");
}
// Create root_ node
root_ = new octree_base_node<Container, PointT> (rootname, NULL, load_all);
// Set root_ nodes tree to the newly created tree
root_->m_tree_ = this;
// Set root_ nodes file path
treepath_ = rootname.parent_path () / (boost::filesystem::basename (rootname) + TREE_EXTENSION_);
loadFromFile ();
}
template <typename PointT> void
pcl::common::mirrorRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
const size_t& amount)
{
if (amount <= 0 || amount > (input.height/2))
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::mirrorRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
uint32_t old_height = input.height;
uint32_t new_height = old_height + 2*amount;
uint32_t old_width = input.width;
if (&input != &output)
output = input;
output.reserve (new_height * old_width);
for(size_t i = 0; i < amount; i++)
{
typename PointCloud<PointT>::iterator up;
if (output.height % 2 == 0)
up = output.begin () + (2*i) * old_width;
else
up = output.begin () + (2*i+1) * old_width;
output.insert (output.begin (), up, up + old_width);
typename PointCloud<PointT>::iterator bottom = output.end () - (2*i+1) * old_width;
output.insert (output.end (), bottom, bottom + old_width);
}
output.width = old_width;
output.height = new_height;
}
void
pcl::GaussianKernel::compute (float sigma,
Eigen::VectorXf &kernel,
unsigned kernel_width) const
{
assert (kernel_width %2 == 1);
assert (sigma >= 0);
kernel.resize (kernel_width);
static const float factor = 0.01f;
static const float max_gauss = 1.0f;
const int hw = kernel_width / 2;
for (int i = -hw, j = 0, k = kernel_width - 1; i < 0 ; i++, j++, k--)
kernel[k] = kernel[j] = float (exp (-i*i / (2*sigma*sigma)));
kernel[hw] = 1;
unsigned g_width = kernel_width;
for (unsigned i = 0; fabs (kernel[i]/max_gauss) < factor; i++, g_width-= 2);
if (g_width == kernel_width)
{
PCL_THROW_EXCEPTION (pcl::KernelWidthTooSmallException,
"kernel width " << kernel_width
<< "is too small for the given sigma " << sigma);
return;
}
// Shift and resize if width less than kernel_width
unsigned shift = (kernel_width - g_width)/2;
for (unsigned i =0; i < g_width; i++)
kernel[i] = kernel[i + shift];
kernel.conservativeResize (g_width);
// Normalize
kernel/= kernel.sum ();
}
template<typename PointT> inline void
pcl::PCA<PointT>::reconstruct (const PointCloud& projection, PointCloud& input)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
if (input.is_dense)
{
input.resize (projection.size ());
for (size_t i = 0; i < projection.size (); ++i)
reconstruct (projection[i], input[i]);
}
else
{
PointT p;
for (size_t i = 0; i < input.size (); ++i)
{
if (!pcl_isfinite (input[i].x) ||
!pcl_isfinite (input[i].y) ||
!pcl_isfinite (input[i].z))
continue;
reconstruct (projection[i], p);
input.push_back (p);
}
}
}
vector<string>
getPcdFilesInDir(const string& directory)
{
namespace fs = boost::filesystem;
fs::path dir(directory);
std::cout << "path: " << directory << std::endl;
if (directory.empty() || !fs::exists(dir) || !fs::is_directory(dir))
PCL_THROW_EXCEPTION (pcl::IOException, "No valid PCD directory given!\n");
vector<string> result;
fs::directory_iterator pos(dir);
fs::directory_iterator end;
for(; pos != end ; ++pos)
if (fs::is_regular_file(pos->status()) )
if (fs::extension(*pos) == ".pcd")
{
#if BOOST_FILESYSTEM_VERSION == 3
result.push_back (pos->path ().string ());
#else
result.push_back (pos->path ());
#endif
cout << "added: " << result.back() << endl;
}
return result;
}
pcl::OpenNIGrabber::OpenNIGrabber (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode)
: rgb_sync_ ()
, ir_sync_ ()
, device_ ()
, rgb_frame_id_ ()
, depth_frame_id_ ()
, image_width_ ()
, image_height_ ()
, depth_width_ ()
, depth_height_ ()
, image_required_ (false)
, depth_required_ (false)
, ir_required_ (false)
, sync_required_ (false)
, image_signal_ (), depth_image_signal_ (), ir_image_signal_ (), image_depth_image_signal_ ()
, ir_depth_image_signal_ (), point_cloud_signal_ (), point_cloud_i_signal_ ()
, point_cloud_rgb_signal_ (), point_cloud_rgba_signal_ ()
, config2xn_map_ (), depth_callback_handle (), image_callback_handle (), ir_callback_handle ()
, running_ (false)
, rgb_array_size_ (0)
, depth_buffer_size_ (0)
, rgb_focal_length_x_ (std::numeric_limits<double>::quiet_NaN ())
, rgb_focal_length_y_ (std::numeric_limits<double>::quiet_NaN ())
, rgb_principal_point_x_ (std::numeric_limits<double>::quiet_NaN ())
, rgb_principal_point_y_ (std::numeric_limits<double>::quiet_NaN ())
, depth_focal_length_x_ (std::numeric_limits<double>::quiet_NaN ())
, depth_focal_length_y_ (std::numeric_limits<double>::quiet_NaN ())
, depth_principal_point_x_ (std::numeric_limits<double>::quiet_NaN ())
, depth_principal_point_y_ (std::numeric_limits<double>::quiet_NaN ())
{
// initialize driver
onInit (device_id, depth_mode, image_mode);
if (!device_->hasDepthStream ())
PCL_THROW_EXCEPTION (pcl::IOException, "Device does not provide 3D information.");
depth_image_signal_ = createSignal<sig_cb_openni_depth_image> ();
ir_image_signal_ = createSignal<sig_cb_openni_ir_image> ();
point_cloud_signal_ = createSignal<sig_cb_openni_point_cloud> ();
point_cloud_i_signal_ = createSignal<sig_cb_openni_point_cloud_i> ();
ir_depth_image_signal_ = createSignal<sig_cb_openni_ir_depth_image> ();
ir_sync_.addCallback (boost::bind (&OpenNIGrabber::irDepthImageCallback, this, _1, _2));
if (device_->hasImageStream ())
{
// create callback signals
image_signal_ = createSignal<sig_cb_openni_image> ();
image_depth_image_signal_ = createSignal<sig_cb_openni_image_depth_image> ();
point_cloud_rgb_signal_ = createSignal<sig_cb_openni_point_cloud_rgb> ();
point_cloud_rgba_signal_ = createSignal<sig_cb_openni_point_cloud_rgba> ();
rgb_sync_.addCallback (boost::bind (&OpenNIGrabber::imageDepthImageCallback, this, _1, _2));
openni_wrapper::DeviceKinect* kinect = dynamic_cast<openni_wrapper::DeviceKinect*> (device_.get ());
if (kinect)
kinect->setDebayeringMethod (openni_wrapper::ImageBayerGRBG::EdgeAware);
}
image_callback_handle = device_->registerImageCallback (&OpenNIGrabber::imageCallback, *this);
depth_callback_handle = device_->registerDepthCallback (&OpenNIGrabber::depthCallback, *this);
ir_callback_handle = device_->registerIRCallback (&OpenNIGrabber::irCallback, *this);
}
template <typename PointOperatorsType> void
pcl::common::Convolution<PointOperatorsType>::initCompute ()
{
if(kernel_width_ % 2 == 0)
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::Convolution::initCompute] convolving element width must be odd.");
half_width_ = kernel_width_ / 2;
switch (borders_policy_)
{
case IGNORE : break;
case MIRROR :
{
spring_.setInputCloud (input_);
spring_.setAmount (half_width_);
spring_.setExpandPolicy (borders_policy_);
spring_.setDirection (convolve_direction_);
spring_.expand ();
} break;
case DUPLICATE :
{
spring_.setInputCloud (input_);
spring_.setAmount (half_width_);
spring_.setExpandPolicy (borders_policy_);
spring_.setDirection (convolve_direction_);
spring_.expand ();
} break;
default:
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::Convolution::initCompute] unknown border policy");
break;
}
if (&(*input_) != &(*output_))
{
if (output_->height < input_->height || output_->width < input_->width)
{
output_->resize (input_->width * input_->height);
output_->width = input_->width;
output_->height = input_->height;
}
}
else
input_.reset (new PointCloudOut (*input_));
}
template<typename Container, typename PointT> boost::uint64_t
octree_base<Container, PointT>::addPointCloud_and_genLOD (PointCloudConstPtr point_cloud)
{
PCL_ERROR ("[pcl::outofcore::octree_base::%s] Function not yet implemented to support point clouds + gen LOD with new PCD dat node files\n",__FUNCTION__);
PCL_THROW_EXCEPTION ( PCLException, "Function not implemented");
//and keep the compiler happy
return 0;// return addDataToLeaf_and_genLOD (point_cloud->points);
}
void
pcl::GaussianKernel::compute (float sigma,
Eigen::VectorXf &kernel,
Eigen::VectorXf &derivative,
unsigned kernel_width) const
{
assert (kernel_width %2 == 1);
assert (sigma >= 0);
kernel.resize (kernel_width);
derivative.resize (kernel_width);
const float factor = 0.01f;
float max_gauss = 1.0f, max_deriv = float (sigma * exp (-0.5));
int hw = kernel_width / 2;
float sigma_sqr = 1.0f / (2.0f * sigma * sigma);
for (int i = -hw, j = 0, k = kernel_width - 1; i < 0 ; i++, j++, k--)
{
kernel[k] = kernel[j] = expf (-static_cast<float>(i) * static_cast<float>(i) * sigma_sqr);
derivative[j] = -static_cast<float>(i) * kernel[j];
derivative[k] = -derivative[j];
}
kernel[hw] = 1;
derivative[hw] = 0;
// Compute kernel and derivative true width
unsigned g_width = kernel_width;
unsigned d_width = kernel_width;
for (unsigned i = 0; fabs (derivative[i]/max_deriv) < factor; i++, d_width-= 2) ;
for (unsigned i = 0; fabs (kernel[i]/max_gauss) < factor; i++, g_width-= 2) ;
if (g_width == kernel_width || d_width == kernel_width)
{
PCL_THROW_EXCEPTION (KernelWidthTooSmallException,
"kernel width " << kernel_width
<< "is too small for the given sigma " << sigma);
return;
}
// Shift and resize if width less than kernel_width
// Kernel
unsigned shift = (kernel_width - g_width)/2;
for (unsigned i =0; i < g_width; i++)
kernel[i] = kernel[i + shift];
// Normalize kernel
kernel.conservativeResize (g_width);
kernel/= kernel.sum ();
// Derivative
shift = (kernel_width - d_width)/2;
for (unsigned i =0; i < d_width; i++)
derivative[i] = derivative[i + shift];
derivative.conservativeResize (d_width);
// Normalize derivative
hw = d_width / 2;
float den = 0;
for (int i = -hw ; i <= hw ; i++)
den -= static_cast<float>(i) * derivative[i+hw];
derivative/= den;
}
template<typename PointT> inline void
pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
}
template<typename PointT> inline void
pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
{
if(!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
input.getVector3fMap ()+= mean_.head<3> ();
}
octree_base<Container, PointT>::octree_base (const double min[3], const double max[3], const double node_dim_meters, const boost::filesystem::path& rootname, const std::string& coord_sys)
: root_ ()
, read_write_mutex_ ()
, lodPoints_ ()
, max_depth_ ()
, treepath_ ()
, coord_system_ ()
, resolution_ ()
{
if (boost::filesystem::exists (rootname.parent_path ()))
{
PCL_ERROR ("[pcl::outofcore::octree_base] A dir named %s already exists. Overwriting an existing tree is not supported.\n", rootname.parent_path ().c_str () );
PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::octree_base] Directory exists; Overwriting an existing tree is not supported\n");
}
// Check file extension
if (boost::filesystem::extension (rootname) != octree_base_node<Container, PointT>::node_index_extension)
{
PCL_ERROR ( "[pcl::outofcore::octree_base] The tree must be created with a root node ending in .oct_idx\n" );
PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::octree_base] Root file extension does not match .oct_idx\n");
}
coord_system_ = coord_sys;
// Get fullpath and recreate directories
boost::filesystem::path dir = boost::filesystem::system_complete (rootname.parent_path ());
boost::filesystem::remove_all (dir);
boost::filesystem::create_directory (dir);
// Create root_ node
root_ = new octree_base_node<Container, PointT> (min, max, node_dim_meters, this, rootname);
root_->m_tree_ = this;
root_->saveIdx (false);
// max_depth_ is set when creating the root_ node
lodPoints_.resize (max_depth_ + 1);
// Set root_ nodes file path
treepath_ = dir / (boost::filesystem::basename (rootname) + TREE_EXTENSION_);
this->saveToFile ();
}
void
pcl::OpenNIGrabber::stopSynchronization ()
{
try
{
if (device_->isSynchronizationSupported () && device_->isSynchronized ())
device_->setSynchronization (false);
}
catch (const openni_wrapper::OpenNIException& exception)
{
PCL_THROW_EXCEPTION (pcl::IOException, "Could not start synchronization " << exception.what ());
}
}
template<typename PointT> bool
pcl::PCA<PointT>::initCompute ()
{
if(!Base::initCompute ())
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
return (false);
}
if(indices_->size () < 3)
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
return (false);
}
// Compute mean
mean_ = Eigen::Vector4f::Zero ();
compute3DCentroid (*input_, *indices_, mean_);
// Compute demeanished cloud
Eigen::MatrixXf cloud_demean;
demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
assert (cloud_demean.cols () == int (indices_->size ()));
// Compute the product cloud_demean * cloud_demean^T
Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
// Compute eigen vectors and values
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
// Organize eigenvectors and eigenvalues in ascendent order
for (int i = 0; i < 3; ++i)
{
eigenvalues_[i] = evd.eigenvalues () [2-i];
eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
}
// If not basis only then compute the coefficients
if (!basis_only_)
coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
compute_done_ = true;
return (true);
}
void
pcl::OpenNIGrabber::signalsChanged ()
{
// reevaluate which streams are required
checkImageStreamRequired ();
checkDepthStreamRequired ();
checkIRStreamRequired ();
if (ir_required_ && image_required_)
PCL_THROW_EXCEPTION (pcl::IOException, "Can not provide IR stream and RGB stream at the same time.");
checkImageAndDepthSynchronizationRequired ();
if (running_)
start ();
}
void
pcl::OpenNIGrabber::startSynchronization ()
{
try
{
if (device_->isSynchronizationSupported () && !device_->isSynchronized () &&
device_->getImageOutputMode ().nFPS == device_->getDepthOutputMode ().nFPS &&
device_->isImageStreamRunning () && device_->isDepthStreamRunning ())
device_->setSynchronization (true);
}
catch (const openni_wrapper::OpenNIException& exception)
{
PCL_THROW_EXCEPTION (pcl::IOException, "Could not start synchronization " << exception.what ());
}
}
template <typename PointT> void
pcl::common::deleteRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
const size_t& amount)
{
if (amount <= 0 || amount > (input.height/2))
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::deleteRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
uint32_t old_height = input.height;
uint32_t old_width = input.width;
output.erase (output.begin (), output.begin () + amount * old_width);
output.erase (output.end () - amount * old_width, output.end ());
output.height = old_height - 2*amount;
output.width = old_width;
}
template <typename PointT> void
pcl::common::expandRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
const PointT& val, const size_t& amount)
{
if (amount <= 0)
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::expandRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
uint32_t old_height = input.height;
uint32_t new_height = old_height + 2*amount;
uint32_t old_width = input.width;
if (&input != &output)
output = input;
output.reserve (new_height * old_width);
output.insert (output.begin (), amount * old_width, val);
output.insert (output.end (), amount * old_width, val);
output.width = old_width;
output.height = new_height;
}
void
pcl::OpenNIGrabber::stop ()
{
try
{
if (device_->hasDepthStream () && device_->isDepthStreamRunning ())
device_->stopDepthStream ();
if (device_->hasImageStream () && device_->isImageStreamRunning ())
device_->stopImageStream ();
if (device_->hasIRStream () && device_->isIRStreamRunning ())
device_->stopIRStream ();
running_ = false;
}
catch (openni_wrapper::OpenNIException& ex)
{
PCL_THROW_EXCEPTION (pcl::IOException, "Could not stop streams. Reason: " << ex.what ());
}
}
pcl::EnsensoGrabber::EnsensoGrabber () :
device_open_ (false),
tcp_open_ (false),
running_ (false)
{
point_cloud_signal_ = createSignal<sig_cb_ensenso_point_cloud> ();
images_signal_ = createSignal<sig_cb_ensenso_images> ();
point_cloud_images_signal_ = createSignal<sig_cb_ensenso_point_cloud_images> ();
PCL_INFO ("Initialising nxLib\n");
try
{
nxLibInitialize ();
root_.reset (new NxLibItem);
}
catch (NxLibException &ex)
{
ensensoExceptionHandling (ex, "EnsensoGrabber");
PCL_THROW_EXCEPTION (pcl::IOException, "Could not initialise NxLib."); // If constructor fails; throw exception
}
}
int
pcl::interpolatePointIndex (int p, int len, InterpolationType type)
{
if (static_cast<unsigned> (p) >= static_cast<unsigned> (len))
{
if (type == BORDER_REPLICATE)
p = p < 0 ? 0 : len - 1;
else if (type == BORDER_REFLECT || type == BORDER_REFLECT_101)
{
int delta = type == BORDER_REFLECT_101;
if (len == 1)
return 0;
do
{
if (p < 0)
p = -p - 1 + delta;
else
p = len - 1 - (p - len) - delta;
}
while (static_cast<unsigned> (p) >= static_cast<unsigned> (len));
}
else if (type == BORDER_WRAP)
{
if (p < 0)
p -= ((p-len+1)/len)*len;
if (p >= len)
p %= len;
}
else if (type == BORDER_CONSTANT)
p = -1;
else
{
PCL_THROW_EXCEPTION (BadArgumentException,
"[pcl::interpolate_point_index] error: Unhandled interpolation type "
<< type << " !");
}
}
return (p);
}
void
pcl::OpenNIGrabber::start ()
{
try
{
// check if we need to start/stop any stream
if (image_required_ && !device_->isImageStreamRunning ())
{
block_signals ();
device_->startImageStream ();
//startSynchronization ();
}
if (depth_required_ && !device_->isDepthStreamRunning ())
{
block_signals ();
if (device_->hasImageStream () && !device_->isDepthRegistered () && device_->isDepthRegistrationSupported ())
{
device_->setDepthRegistration (true);
}
device_->startDepthStream ();
//startSynchronization ();
}
if (ir_required_ && !device_->isIRStreamRunning ())
{
block_signals ();
device_->startIRStream ();
}
running_ = true;
}
catch (openni_wrapper::OpenNIException& ex)
{
PCL_THROW_EXCEPTION (pcl::IOException, "Could not start streams. Reason: " << ex.what ());
}
// workaround, since the first frame is corrupted
boost::this_thread::sleep (boost::posix_time::seconds (1));
unblock_signals ();
}
template <typename PointT> void
pcl::common::duplicateRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
const size_t& amount)
{
if (amount <= 0 || amount > (input.height/2))
PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::common::duplicateRows] error: amount must be ]0.."
<< (input.height/2) << "] !");
uint32_t old_height = input.height;
uint32_t new_height = old_height + 2*amount;
uint32_t old_width = input.width;
if (&input != &output)
output = input;
output.reserve (new_height * old_width);
for(size_t i = 0; i < amount; ++i)
{
output.insert (output.begin (), output.begin (), output.begin () + old_width);
output.insert (output.end (), output.end () - old_width, output.end ());
}
output.width = old_width;
output.height = new_height;
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0; i<eigenvalues_.size(); i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
