template <typename PointSource, typename PointTarget> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
const std::vector<int> &indices_src,
const PointCloudTarget &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () < 4) // need at least 4 samples
{
PCL_THROW_EXCEPTION (NotEnoughPointsException,
"[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
return;
}
// Set the initial solution
Vector6d x = Vector6d::Zero ();
x[0] = transformation_matrix (0,3);
x[1] = transformation_matrix (1,3);
x[2] = transformation_matrix (2,3);
x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
x[4] = asin (-transformation_matrix (2,0));
x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
tmp_idx_src_ = &indices_src;
tmp_idx_tgt_ = &indices_tgt;
// Optimize using forward-difference approximation LM
const double gradient_tol = 1e-2;
OptimizationFunctorWithIndices functor(this);
BFGS<OptimizationFunctorWithIndices> bfgs (functor);
bfgs.parameters.sigma = 0.01;
bfgs.parameters.rho = 0.01;
bfgs.parameters.tau1 = 9;
bfgs.parameters.tau2 = 0.05;
bfgs.parameters.tau3 = 0.5;
bfgs.parameters.order = 3;
int inner_iterations_ = 0;
int result = bfgs.minimizeInit (x);
result = BFGSSpace::Running;
do
{
inner_iterations_++;
result = bfgs.minimizeOneStep (x);
if(result)
{
break;
}
result = bfgs.testGradient(gradient_tol);
} while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
{
PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
transformation_matrix.setIdentity();
applyState(transformation_matrix, x);
}
else
PCL_THROW_EXCEPTION(SolverDidntConvergeException,
"[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
}
template <typename PointSource, typename PointTarget> inline void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
using namespace std;
// Difference between consecutive transforms
double delta = 0;
// Get the size of the target
const size_t N = indices_->size ();
// Set the mahalanobis matrices to identity
mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
// Compute target cloud covariance matrices
if ((!target_covariances_) || (target_covariances_->empty ()))
{
target_covariances_.reset (new MatricesVector);
computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
}
// Compute input cloud covariance matrices
if ((!input_covariances_) || (input_covariances_->empty ()))
{
input_covariances_.reset (new MatricesVector);
computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
}
base_transformation_ = guess;
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
while(!converged_)
{
size_t cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// guess corresponds to base_t and transformation_ to t
Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
for(size_t i = 0; i < 4; i++)
for(size_t j = 0; j < 4; j++)
for(size_t k = 0; k < 4; k++)
transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
for (size_t i = 0; i < N; i++)
{
PointSource query = output[i];
query.getVector4fMap () = guess * query.getVector4fMap ();
query.getVector4fMap () = transformation_ * query.getVector4fMap ();
if (!searchForNeighbors (query, nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
Eigen::Matrix3d &C1 = (*input_covariances_)[i];
Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
Eigen::Matrix3d &M = mahalanobis_[i];
// M = R*C1
M = R * C1;
// temp = M*R' + C2 = R*C1*R' + C2
Eigen::Matrix3d temp = M * R.transpose();
temp+= C2;
// M = temp^-1
M = temp.inverse ();
source_indices[cnt] = static_cast<int> (i);
target_indices[cnt] = nn_indices[0];
cnt++;
}
}
// Resize to the actual number of valid correspondences
source_indices.resize(cnt); target_indices.resize(cnt);
/* optimize transformation using the current assignment and Mahalanobis metrics*/
previous_transformation_ = transformation_;
//optimization right here
try
{
rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
/* compute the delta from this iteration */
delta = 0.;
for(int k = 0; k < 4; k++) {
for(int l = 0; l < 4; l++) {
double ratio = 1;
if(k < 3 && l < 3) // rotation part of the transform
ratio = 1./rotation_epsilon_;
else
ratio = 1./transformation_epsilon_;
double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
if(c_delta > delta)
delta = c_delta;
}
}
}
catch (PCLException &e)
{
PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
break;
}
nr_iterations_++;
// Check for convergence
if (nr_iterations_ >= max_iterations_ || delta < 1)
{
converged_ = true;
previous_transformation_ = transformation_;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
}
else
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
}
//for some reason the static equivalent methode raises an error
// final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
// final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
// Transform the point cloud
pcl::transformPointCloud (*input_, output, final_transformation_);
}
template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl16::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
if (!input_features_)
{
PCL16_ERROR ("[pcl16::%s::computeTransformation] ", getClassName ().c_str ());
PCL16_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
return;
}
if (!target_features_)
{
PCL16_ERROR ("[pcl16::%s::computeTransformation] ", getClassName ().c_str ());
PCL16_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
return;
}
if (!error_functor_)
{
error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_)));
}
std::vector<int> sample_indices (nr_samples_);
std::vector<int> corresponding_indices (nr_samples_);
PointCloudSource input_transformed;
float error, lowest_error (0);
final_transformation_ = guess;
int i_iter = 0;
if (!guess.isApprox(Eigen::Matrix4f::Identity (), 0.01f))
{ //If guess is not the Identity matrix we check it.
transformPointCloud (*input_, input_transformed, final_transformation_);
lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
i_iter = 1;
}
for (; i_iter < max_iterations_; ++i_iter)
{
// Draw nr_samples_ random samples
selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
// Find corresponding features in the target cloud
findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
// Estimate the transform from the samples to their corresponding points
transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
// Tranform the data and compute the error
transformPointCloud (*input_, input_transformed, transformation_);
error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
// If the new error is lower, update the final transformation
if (i_iter == 0 || error < lowest_error)
{
lowest_error = error;
final_transformation_ = transformation_;
}
}
// Apply the final transformation
transformPointCloud (*input_, output, final_transformation_);
}
/** \brief Align a pair of PointCloud datasets and return the result
* \param cloud_src the source PointCloud
* \param cloud_tgt the target PointCloud
* \param output the resultant aligned source PointCloud
* \param final_transform the resultant transform between source and target
*/
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
//
// Downsample for consistency and speed
// \note enable this for large datasets
PointCloud::Ptr src (new PointCloud);
PointCloud::Ptr tgt (new PointCloud);
pcl::VoxelGrid<PointT> grid;
if (downsample)
{
grid.setLeafSize (0.005, 0.005, 0.005);
grid.setInputCloud (cloud_src);
grid.filter (*src);
grid.setInputCloud (cloud_tgt);
grid.filter (*tgt);
}
else
{
src = cloud_src;
tgt = cloud_tgt;
}
// Compute surface normals and curvature
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est;
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
norm_est.setSearchMethod (tree);
norm_est.setKSearch (30);
norm_est.setInputCloud (src);
norm_est.compute (*points_with_normals_src);
pcl::copyPointCloud (*src, *points_with_normals_src);
norm_est.setInputCloud (tgt);
norm_est.compute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);
//
// Instantiate our custom point representation (defined above) ...
MyPointRepresentation point_representation;
// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);
float n;
//float it;
//PCL_INFO("Set the max correspondence distance: ");
//std::cin>> n;
//PCL_INFO("Set the maximum number of iterations: ");
//std::cin>> it;
//PCL_INFO("Set the reciprocal correspondences: ");
//char d;
//std::cin >> d;
// Align
pcl::IterativeClosestPointWithNormals<PointNormalT, PointNormalT> reg; //IterativeClosestPointNonLinear
reg.setTransformationEpsilon (1e-8);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance (0.1); //n
//reg.setEuclideanFitnessEpsilon (1e-15);
//reg.setRANSACIterations(200);
reg.setUseReciprocalCorrespondences(true);
reg.setRANSACOutlierRejectionThreshold(0.01);
// Set the point representation
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputSource (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
//
// Run the same optimization in a loop and visualize the results
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations (2);
for (int i = 0; i < 100; ++i) //it
{
PCL_INFO ("Iteration Nr. %d.\n", i);
// save cloud for visualization purpose
points_with_normals_src = reg_result;
// Estimate
reg.setInputSource (points_with_normals_src);
reg.align (*reg_result);
//accumulate transformation between each Iteration
Ti = reg.getFinalTransformation () * Ti;
//if the difference between this transformation and the previous one
//is smaller than the threshold, refine the process by reducing
//the maximal correspondence distance
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
prev = reg.getLastIncrementalTransformation ();
// visualize current state
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
//
// Get the transformation from target to source
targetToSource = Ti.inverse();
//
// Transform target back in source frame
pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);
p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");
//add the source to the transformed target
*output += *cloud_src;
final_transform = targetToSource;
}
void
pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::recognize ()
{
models_.reset (new std::vector<ModelT>);
transforms_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
PointInTPtr processed;
typename pcl::PointCloud<FeatureT>::Ptr signatures (new pcl::PointCloud<FeatureT> ());
//pcl::PointCloud<int> keypoints_input;
PointInTPtr keypoints_pointcloud;
if (signatures_ != 0 && processed_ != 0 && (signatures_->size () == keypoints_pointcloud->points.size ()))
{
keypoints_pointcloud = keypoints_input_;
signatures = signatures_;
processed = processed_;
std::cout << "Using the ISPK ..." << std::endl;
}
else
{
processed.reset( (new pcl::PointCloud<PointInT>));
if (indices_.size () > 0)
{
PointInTPtr sub_input (new pcl::PointCloud<PointInT>);
pcl::copyPointCloud (*input_, indices_, *sub_input);
estimator_->estimate (sub_input, processed, keypoints_pointcloud, signatures);
}
else
{
estimator_->estimate (input_, processed, keypoints_pointcloud, signatures);
}
processed_ = processed;
}
std::cout << "Number of keypoints:" << keypoints_pointcloud->points.size () << std::endl;
int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
//feature matching and object hypotheses
std::map<std::string, ObjectHypothesis> object_hypotheses;
{
for (size_t idx = 0; idx < signatures->points.size (); idx++)
{
float* hist = signatures->points[idx].histogram;
std::vector<float> std_hist (hist, hist + size_feat);
flann_model histogram;
histogram.descr = std_hist;
flann::Matrix<int> indices;
flann::Matrix<float> distances;
nearestKSearch (flann_index_, histogram, 1, indices, distances);
//read view pose and keypoint coordinates, transform keypoint coordinates to model coordinates
Eigen::Matrix4f homMatrixPose;
getPose (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, homMatrixPose);
typename pcl::PointCloud<PointInT>::Ptr keypoints (new pcl::PointCloud<PointInT> ());
getKeypoints (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, keypoints);
PointInT view_keypoint = keypoints->points[flann_models_.at (indices[0][0]).keypoint_id];
PointInT model_keypoint;
model_keypoint.getVector4fMap () = homMatrixPose.inverse () * view_keypoint.getVector4fMap ();
typename std::map<std::string, ObjectHypothesis>::iterator it_map;
if ((it_map = object_hypotheses.find (flann_models_.at (indices[0][0]).model.id_)) != object_hypotheses.end ())
{
//if the object hypothesis already exists, then add information
ObjectHypothesis oh = (*it_map).second;
oh.correspondences_pointcloud->points.push_back (model_keypoint);
oh.correspondences_to_inputcloud->push_back (
pcl::Correspondence (static_cast<int> (oh.correspondences_pointcloud->points.size () - 1),
static_cast<int> (idx), distances[0][0]));
oh.feature_distances_->push_back (distances[0][0]);
}
else
{
//create object hypothesis
ObjectHypothesis oh;
typename pcl::PointCloud<PointInT>::Ptr correspondences_pointcloud (new pcl::PointCloud<PointInT> ());
correspondences_pointcloud->points.push_back (model_keypoint);
oh.model_ = flann_models_.at (indices[0][0]).model;
oh.correspondences_pointcloud = correspondences_pointcloud;
//last keypoint for this model is a correspondence the current scene keypoint
pcl::CorrespondencesPtr corr (new pcl::Correspondences ());
oh.correspondences_to_inputcloud = corr;
oh.correspondences_to_inputcloud->push_back (pcl::Correspondence (0, static_cast<int> (idx), distances[0][0]));
boost::shared_ptr < std::vector<float> > feat_dist (new std::vector<float>);
feat_dist->push_back (distances[0][0]);
oh.feature_distances_ = feat_dist;
object_hypotheses[oh.model_.id_] = oh;
}
}
}
typename std::map<std::string, ObjectHypothesis>::iterator it_map;
std::vector<float> feature_distance_avg;
{
//pcl::ScopeTime t("Geometric verification, RANSAC and transform estimation");
for (it_map = object_hypotheses.begin (); it_map != object_hypotheses.end (); it_map++)
{
std::vector < pcl::Correspondences > corresp_clusters;
cg_algorithm_->setSceneCloud (keypoints_pointcloud);
cg_algorithm_->setInputCloud ((*it_map).second.correspondences_pointcloud);
cg_algorithm_->setModelSceneCorrespondences ((*it_map).second.correspondences_to_inputcloud);
cg_algorithm_->cluster (corresp_clusters);
std::cout << "Instances:" << corresp_clusters.size () << " Total correspondences:" << (*it_map).second.correspondences_to_inputcloud->size () << " " << it_map->first << std::endl;
std::vector<bool> good_indices_for_hypothesis (corresp_clusters.size (), true);
if (threshold_accept_model_hypothesis_ < 1.f)
{
//sort the hypotheses for each model according to their correspondences and take those that are threshold_accept_model_hypothesis_ over the max cardinality
int max_cardinality = -1;
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
//std::cout << (corresp_clusters[i]).size() << " -- " << (*(*it_map).second.correspondences_to_inputcloud).size() << std::endl;
if (max_cardinality < static_cast<int> (corresp_clusters[i].size ()))
{
max_cardinality = static_cast<int> (corresp_clusters[i].size ());
}
}
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
if (static_cast<float> ((corresp_clusters[i]).size ()) < (threshold_accept_model_hypothesis_ * static_cast<float> (max_cardinality)))
{
good_indices_for_hypothesis[i] = false;
}
}
}
for (size_t i = 0; i < corresp_clusters.size (); i++)
{
if (!good_indices_for_hypothesis[i])
continue;
//drawCorrespondences (processed, it_map->second, keypoints_pointcloud, corresp_clusters[i]);
Eigen::Matrix4f best_trans;
typename pcl::registration::TransformationEstimationSVD < PointInT, PointInT > t_est;
t_est.estimateRigidTransformation (*(*it_map).second.correspondences_pointcloud, *keypoints_pointcloud, corresp_clusters[i], best_trans);
models_->push_back ((*it_map).second.model_);
transforms_->push_back (best_trans);
}
}
}
std::cout << "Number of hypotheses:" << models_->size() << std::endl;
/**
* POSE REFINEMENT
**/
if (ICP_iterations_ > 0)
{
pcl::ScopeTime ticp ("ICP ");
//Prepare scene and model clouds for the pose refinement step
PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
pcl::VoxelGrid<PointInT> voxel_grid_icp;
voxel_grid_icp.setInputCloud (processed);
voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
voxel_grid_icp.filter (*cloud_voxelized_icp);
source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
#pragma omp parallel for schedule(dynamic,1) num_threads(omp_get_num_procs())
for (int i = 0; i < static_cast<int>(models_->size ()); i++)
{
ConstPointInTPtr model_cloud;
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT>::Ptr rej (
new pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT> ());
rej->setInputTarget (cloud_voxelized_icp);
rej->setMaximumIterations (1000);
rej->setInlierThreshold (0.005f);
rej->setInputSource (model_aligned);
pcl::IterativeClosestPoint<PointInT, PointInT> reg;
reg.addCorrespondenceRejector (rej);
reg.setInputTarget (cloud_voxelized_icp); //scene
reg.setInputSource (model_aligned); //model
reg.setMaximumIterations (ICP_iterations_);
reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 4.f);
typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
reg.align (*output_);
Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
transforms_->at (i) = icp_trans * transforms_->at (i);
}
}
/**
* HYPOTHESES VERIFICATION
**/
if (hv_algorithm_)
{
pcl::ScopeTime thv ("HV verification");
std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
aligned_models.resize (models_->size ());
for (size_t i = 0; i < models_->size (); i++)
{
ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.0025f);
//ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
aligned_models[i] = model_aligned;
}
std::vector<bool> mask_hv;
hv_algorithm_->setSceneCloud (input_);
hv_algorithm_->addModels (aligned_models, true);
hv_algorithm_->verify ();
hv_algorithm_->getMask (mask_hv);
boost::shared_ptr < std::vector<ModelT> > models_temp;
boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_temp;
models_temp.reset (new std::vector<ModelT>);
transforms_temp.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
for (size_t i = 0; i < models_->size (); i++)
{
if (!mask_hv[i])
continue;
models_temp->push_back (models_->at (i));
transforms_temp->push_back (transforms_->at (i));
}
models_ = models_temp;
transforms_ = transforms_temp;
}
}
void set_uniform_matrix(GLuint programID, const char* NAME, const Eigen::Matrix4f& matrix) {
GLuint matrix_id = glGetUniformLocation(programID, NAME);
glUniformMatrix4fv(matrix_id, 1, GL_FALSE, matrix.data());
}
Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
p2w.block<3,3>(0,0) = eigDx.transpose();
p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
pcl::PointCloud<PointN> cPoints;
pcl::transformPointCloud(*final, cPoints, p2w);
PointN min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
// final transform
const Eigen::Quaternionf qfinal(eigDx);
tfinal = eigDx*mean_diag + centroid.head<3>();
std::cout << "tfinal: " << tfinal << std::endl;
pcl::PointXYZRGB minp, maxp;
Eigen::Matrix4f _tr = Eigen::Matrix4f::Identity();
_tr.topLeftCorner<3,3>() = qfinal.toRotationMatrix();
_tr.block<3,1>(0,3) = tfinal;
_x = (max_pt.x-min_pt.x);// * 0.5;
_y = (max_pt.y-min_pt.y);// * 0.5;
_z = (max_pt.z-min_pt.z);// * 0.5;
_tr = _tr.inverse().eval();
qfinal_r = qfinal;
pcl::PointIndices::Ptr object_indices (new pcl::PointIndices);
for (size_t i = 0; i < final->size(); i++){
PointN p = (*final)[i];
void GenericIKWindow::solve()
{
if (!kc || !ikSolver || !tcp)
{
return;
}
cout << "---- Solve IK ----" << endl;
IKSolver::CartesianSelection s = IKSolver::All;
if (UI.radioButton_Pos->isChecked())
{
s = IKSolver::Position;
}
//if (UI.radioButton_Ori->isChecked())
// s = IKSolver::Orientation;
//ikSolver->setVerbose(true);
Eigen::Matrix4f targetPose = box->getGlobalPose();
/*
if (kc && kc->getNode(kc->getSize() - 1)->isTranslationalJoint() && kc->getNode(kc->getSize() - 1)->getParent())
{
// setup gaze IK
float v = (kc->getNode(kc->getSize() - 1)->getParent()->getGlobalPose().block(0, 3, 3, 1) - targetPose.block(0, 3, 3, 1)).norm();
cout << "Setting initial value of translation joint to :" << v << endl;
ikSolver->setupTranslationalJoint(kc->getNode(kc->getSize() - 1), v);
kc->getNode(kc->getSize() - 1)->setJointValue(v);
}*/
clock_t startT = clock();
if (ikGazeSolver)
{
ikGazeSolver->solve(targetPose.block(0, 3, 3, 1));
}
else
{
ikSolver->solve(targetPose, s, 50);
}
clock_t endT = clock();
Eigen::Matrix4f actPose = tcp->getGlobalPose();
float errorPos = (actPose.block(0, 3, 3, 1) - targetPose.block(0, 3, 3, 1)).norm();
MathTools::Quaternion q1 = MathTools::eigen4f2quat(actPose);
MathTools::Quaternion q2 = MathTools::eigen4f2quat(targetPose);
MathTools::Quaternion d = getDelta(q1, q2);
float errorOri = fabs(180.0f - (d.w + 1.0f) * 90.0f);
float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
QString qd = "Time: ";
qd += QString::number(diffClock, 'f', 2);
qd += " ms";
UI.labelTime->setText(qd);
QString qd2 = "Error Pos: : ";
qd2 += QString::number(errorPos, 'f', 2);
qd2 += " mm";
UI.labelPos->setText(qd2);
QString qd3 = "Error Ori: : ";
qd3 += QString::number(errorOri, 'f', 2);
qd3 += " deg";
UI.labelOri->setText(qd3);
cout << "Joint values:" << endl;
std::vector<RobotNodePtr> nodes = kc->getAllRobotNodes();
for (size_t i = 0; i < nodes.size(); i++)
{
cout << nodes[i]->getJointValue() << endl;
}
/*
DifferentialIKPtr j(new DifferentialIK(kc));
j->setGoal(targetPose,RobotNodePtr(),IKSolver::All);
j->computeSteps(0.2f,0,50);
*/
exViewer->render();
cout << "---- END Solve IK ----" << endl;
}
void IndexMap::synthesizeInfo(const Eigen::Matrix4f & pose,
const std::pair<GLuint, GLuint> & model,
const float depthCutoff,
const float confThreshold)
{
glEnable(GL_PROGRAM_POINT_SIZE);
glEnable(GL_POINT_SPRITE);
infoFrameBuffer.Bind();
glPushAttrib(GL_VIEWPORT_BIT);
glViewport(0, 0, infoRenderBuffer.width, infoRenderBuffer.height);
glClearColor(0, 0, 0, 0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
combinedProgram->Bind();
Eigen::Matrix4f t_inv = pose.inverse();
Eigen::Vector4f cam(Intrinsics::getInstance().cx(),
Intrinsics::getInstance().cy(),
Intrinsics::getInstance().fx(),
Intrinsics::getInstance().fy());
combinedProgram->setUniform(Uniform("t_inv", t_inv));
combinedProgram->setUniform(Uniform("cam", cam));
combinedProgram->setUniform(Uniform("maxDepth", depthCutoff));
combinedProgram->setUniform(Uniform("confThreshold", confThreshold));
combinedProgram->setUniform(Uniform("cols", (float)Resolution::getInstance().cols()));
combinedProgram->setUniform(Uniform("rows", (float)Resolution::getInstance().rows()));
combinedProgram->setUniform(Uniform("time", 0));
combinedProgram->setUniform(Uniform("maxTime", std::numeric_limits<int>::max()));
combinedProgram->setUniform(Uniform("timeDelta", std::numeric_limits<int>::max()));
glBindBuffer(GL_ARRAY_BUFFER, model.first);
glEnableVertexAttribArray(0);
glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, 0);
glEnableVertexAttribArray(1);
glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f) * 1));
glEnableVertexAttribArray(2);
glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f) * 2));
glDrawTransformFeedback(GL_POINTS, model.second);
glDisableVertexAttribArray(0);
glDisableVertexAttribArray(1);
glDisableVertexAttribArray(2);
glBindBuffer(GL_ARRAY_BUFFER, 0);
infoFrameBuffer.Unbind();
combinedProgram->Unbind();
glDisable(GL_PROGRAM_POINT_SIZE);
glDisable(GL_POINT_SPRITE);
glPopAttrib();
glFinish();
}
template <typename PointT> bool
pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
// Need 4 samples
if (samples.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
return (false);
}
Eigen::Matrix4f temp;
for (int i = 0; i < 4; i++)
{
temp (i, 0) = input_->points[samples[i]].x;
temp (i, 1) = input_->points[samples[i]].y;
temp (i, 2) = input_->points[samples[i]].z;
temp (i, 3) = 1;
}
float m11 = temp.determinant ();
if (m11 == 0)
return (false); // the points don't define a sphere!
for (int i = 0; i < 4; ++i)
temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
(input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
(input_->points[samples[i]].z) * (input_->points[samples[i]].z);
float m12 = temp.determinant ();
for (int i = 0; i < 4; ++i)
{
temp (i, 1) = temp (i, 0);
temp (i, 0) = input_->points[samples[i]].x;
}
float m13 = temp.determinant ();
for (int i = 0; i < 4; ++i)
{
temp (i, 2) = temp (i, 1);
temp (i, 1) = input_->points[samples[i]].y;
}
float m14 = temp.determinant ();
for (int i = 0; i < 4; ++i)
{
temp (i, 0) = temp (i, 2);
temp (i, 1) = input_->points[samples[i]].x;
temp (i, 2) = input_->points[samples[i]].y;
temp (i, 3) = input_->points[samples[i]].z;
}
float m15 = temp.determinant ();
// Center (x , y, z)
model_coefficients.resize (4);
model_coefficients[0] = 0.5f * m12 / m11;
model_coefficients[1] = 0.5f * m13 / m11;
model_coefficients[2] = 0.5f * m14 / m11;
// Radius
model_coefficients[3] = sqrtf (
model_coefficients[0] * model_coefficients[0] +
model_coefficients[1] * model_coefficients[1] +
model_coefficients[2] * model_coefficients[2] - m15 / m11);
return (true);
}
void ICP::Iterate_ICP(const CameraData& d_newDepthData,
const PointInfo& d_renderedVertexMap,
float* d_currentTransform) // This is input & output parameter on cuda
{
//static const Eigen::Matrix4f m_identity4f = Eigen::Matrix4f::Identity();
// Eigen::Matrix4f m_identity4f;
// m_identity4f.setIdentity();
cutilSafeCall( cudaMemcpy(incrementalTransform, m_identity4f.data(), 16*sizeof(float), cudaMemcpyHostToDevice) );
// d_currentTransform currently holds the previous transformation matrix. Initialize new transformation matrix to the previous
cutilSafeCall( cudaMemcpy(newTransform, d_currentTransform, 16*sizeof(float), cudaMemcpyDeviceToDevice) );
int iterationCounter = 0;
cutilSafeCall(cudaMemcpy(bestTransform, newTransform, 16*sizeof(float), cudaMemcpyDeviceToDevice));
// While loop - until we get minimum transformation matrix:
do {
iterationCounter++;
// Copy current transform to its matching field.
cutilSafeCall( cudaMemcpy(invLastTransform_host, newTransform, 16*sizeof(float), cudaMemcpyDeviceToHost) );
//cutilSafeCall( cudaMemcpy(invLastTransformMatrix.data(), newTransform, 16*sizeof(float), cudaMemcpyDeviceToHost) );
cudaDeviceSynchronize();
// Calculate inverse of current transformation matrix and store in invLastTransformMatrix.
Eigen::Map<Eigen::Matrix<float, 4, 4, Eigen::RowMajor> > invLastTransformMatrix(invLastTransform_host); // TODO: COLMAJOR IS ALSO WORKING WELL. CHECK OUT. MAYBE BUGGY
invLastTransformMatrix = invLastTransformMatrix.inverse();
// Copy current inverse transformation matrix into cuda.
cutilSafeCall( cudaMemcpy(invLastTransform, invLastTransform_host, 16*sizeof(float), cudaMemcpyHostToDevice) );
cudaDeviceSynchronize();
// Find corresponding points and build A and b matrices.
FindCorresponding (d_newDepthData, d_renderedVertexMap);
//////calculating Xopt = (At * A)^(-1) * At * b using tree reduction//////
cutilSafeCall(cudaDeviceSynchronize());
ClearHostMatrices();
cutilSafeCall(cudaMemcpy(AtA_host, AtA, NUM_VARS*NUM_VARS*m_numCorrespondenceBlocks*sizeof(float), cudaMemcpyDeviceToHost));
cutilSafeCall(cudaMemcpy(Atb_host, Atb, NUM_VARS*m_numCorrespondenceBlocks*sizeof(float), cudaMemcpyDeviceToHost));
for (int i = 0; i < m_numCorrespondenceBlocks ; ++i) {
for (int j = 0; j < NUM_VARS*NUM_VARS; ++j) {
AtA_sum[j] += AtA_host[i * NUM_VARS*NUM_VARS + j];
}
for (int j = 0; j < NUM_VARS; ++j) {
Atb_sum[j] += Atb_host[i * NUM_VARS + j];
}
}
for (int i = 0; i < NUM_VARS; ++i) {
for (int j = 0; j < i; ++j) {
AtA_sum[i * NUM_VARS + j] = AtA_sum[j * NUM_VARS + i];
}
}
Eigen::Matrix<float, NUM_VARS, NUM_VARS, Eigen::RowMajor> AtA_eigen;
Eigen::Matrix<float, NUM_VARS, 1> Atb_eigen;
for (int i = 0; i < NUM_VARS; ++i) {
Atb_eigen(i) = Atb_sum[i];
for (int j = 0; j < NUM_VARS; ++j) {
AtA_eigen(i, j) = AtA_sum[i * NUM_VARS + j];
}
}
float det = AtA_eigen.determinant();
if (isnan(det) || det == 0.f || fabs(det) < _EPSILON_) {
// TODO - PROBLEM! MATRIX IS SINGULAR. HANDLE
cutilSafeCall( cudaMemcpy(newTransform, d_currentTransform, 16*sizeof(float), cudaMemcpyDeviceToDevice) );
cout << " No transform found." << endl;
break;
}
Eigen::Matrix<float, NUM_VARS, 1> parameters = AtA_eigen.llt().solve(Atb_eigen).cast<float>();
for (int i = 0; i < 3; ++i) {
xOpt_host[i] = -parameters(i); // TODO - CONSIDER WRITE -parameters(i) (minus) like in KinectShape
}
for (int i = 3; i < 6; ++i) { // Angles are negated, translations are positive
xOpt_host[i] = parameters(i);
}
cutilSafeCall(cudaMemcpy(xOpt, xOpt_host, NUM_VARS*sizeof(float), cudaMemcpyHostToDevice));
BuildNewTransformMatrix();
CameraHandler::updateCameraData(m_gridSize, m_blockSize, d_newDepthData, NULL, newTransform, NULL, NULL);
cutilSafeCall(cudaDeviceSynchronize());
} while (iterationCounter < m_maxIterations);
cutilSafeCall(cudaMemcpy(d_currentTransform, newTransform, 16*sizeof(float), cudaMemcpyDeviceToDevice));
// The output is in @Param d_currentTransform
}
void pairAlign( const PointCloud::Ptr cloud_src , const PointCloud::Ptr cloud_tgt , Eigen::Matrix4f &final_transform )
{
PointCloud::Ptr src (new PointCloud);
PointCloud::Ptr tgt (new PointCloud);
*src = *cloud_src;
*tgt = *cloud_tgt;
// Compute surface normals and curvature
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
norm_est.setSearchMethod (tree);
norm_est.setKSearch (30);
norm_est.setInputCloud (src);
norm_est.compute (*points_with_normals_src);
pcl::copyPointCloud (*src, *points_with_normals_src);
norm_est.setInputCloud (tgt);
norm_est.compute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);
// Instantiate our custom point representation (defined above) ...
MyPointRepresentation point_representation;
// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);
//
// Align
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
reg.setTransformationEpsilon (1e-3);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance (0.5);
// Set the point representation
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputSource (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
//
// Run the same optimization in a loop and visualize the results
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations (2);
//originally iterates up to 30
for (int i = 0; i < 10; ++i)
{
// save cloud for visualization purpose
points_with_normals_src = reg_result;
// Estimate
reg.setInputSource (points_with_normals_src);
reg.align (*reg_result);
//accumulate transformation between each Iteration
Ti = reg.getFinalTransformation () * Ti;
//if the difference between this transformation and the previous one
//is smaller than the threshold, refine the process by reducing
//the maximal correspondence distance
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
{
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
}
prev = reg.getLastIncrementalTransformation ();
}
//
// Get the transformation from target to source
targetToSource = Ti.inverse();
final_transform = targetToSource;
}
/* ---[ */
int
main (int argc, char** argv)
{
print_info ("Transform a cloud. For more information, use: %s -h\n", argv[0]);
bool help = false;
parse_argument (argc, argv, "-h", help);
if (argc < 3 || help)
{
printHelp (argc, argv);
return (-1);
}
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices;
p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
if (p_file_indices.size () != 2)
{
print_error ("Need one input PCD file and one output PCD file to continue.\n");
return (-1);
}
// Initialize the transformation matrix
Eigen::Matrix4f tform;
tform.setIdentity ();
// Command line parsing
float dx, dy, dz;
std::vector<float> values;
if (parse_3x_arguments (argc, argv, "-trans", dx, dy, dz) > -1)
{
tform (0, 3) = dx;
tform (1, 3) = dy;
tform (2, 3) = dz;
}
if (parse_x_arguments (argc, argv, "-quat", values) > -1)
{
if (values.size () == 4)
{
const float& x = values[0];
const float& y = values[1];
const float& z = values[2];
const float& w = values[3];
tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::Quaternionf (w, x, y, z));
}
else
{
print_error ("Wrong number of values given (%zu): ", values.size ());
print_error ("The quaternion specified with -quat must contain 4 elements (w,x,y,z).\n");
}
}
if (parse_x_arguments (argc, argv, "-axisangle", values) > -1)
{
if (values.size () == 4)
{
const float& ax = values[0];
const float& ay = values[1];
const float& az = values[2];
const float& theta = values[3];
tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::AngleAxisf (theta, Eigen::Vector3f (ax, ay, az)));
}
else
{
print_error ("Wrong number of values given (%zu): ", values.size ());
print_error ("The rotation specified with -axisangle must contain 4 elements (ax,ay,az,theta).\n");
}
}
if (parse_x_arguments (argc, argv, "-matrix", values) > -1)
{
if (values.size () == 9 || values.size () == 16)
{
int n = values.size () == 9 ? 3 : 4;
for (int r = 0; r < n; ++r)
for (int c = 0; c < n; ++c)
tform (r, c) = values[n*r+c];
}
else
{
print_error ("Wrong number of values given (%zu): ", values.size ());
print_error ("The transformation specified with -matrix must be 3x3 (9) or 4x4 (16).\n");
}
}
// Load the first file
sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
if (!loadCloud (argv[p_file_indices[0]], *cloud))
return (-1);
// Apply the transform
sensor_msgs::PointCloud2 output;
compute (cloud, output, tform);
// Check if a scaling parameter has been given
double divider[3];
if (parse_3x_arguments (argc, argv, "-scale", divider[0], divider[1], divider[2]) > -1)
{
print_highlight ("Scaling XYZ data with the following values: %f, %f, %f\n", divider[0], divider[1], divider[2]);
scaleInPlace (output, divider);
}
// Save into the second file
saveCloud (argv[p_file_indices[1]], output);
}
int volumetric_knt_cuda(int argc, char **argv)
{
Timer timer;
int vol_size = vx_count * vx_size;
float half_vol_size = vol_size * 0.5f;
Eigen::Vector3i voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3i volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3i voxel_count(vx_count, vx_count, vx_count);
int total_voxels = voxel_count.x() * voxel_count.y() * voxel_count.z();
std::cout << std::fixed
<< "Voxel Count : " << voxel_count.transpose() << std::endl
<< "Voxel Size : " << voxel_size.transpose() << std::endl
<< "Volume Size : " << volume_size.transpose() << std::endl
<< "Total Voxels : " << total_voxels << std::endl
<< std::endl;
timer.start();
KinectFrame knt(filepath);
timer.print_interval("Importing knt frame : ");
Eigen::Affine3f grid_affine = Eigen::Affine3f::Identity();
grid_affine.translate(Eigen::Vector3f(0, 0, half_vol_size));
grid_affine.scale(Eigen::Vector3f(1, 1, 1)); // z is negative inside of screen
Eigen::Matrix4f grid_matrix = grid_affine.matrix();
float knt_near_plane = 0.1f;
float knt_far_plane = 10240.0f;
Eigen::Matrix4f projection = perspective_matrix<float>(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, knt_near_plane, knt_far_plane);
Eigen::Matrix4f projection_inverse = projection.inverse();
Eigen::Matrix4f view_matrix = Eigen::Matrix4f::Identity();
std::vector<float4> vertices(knt.depth.size(), make_float4(0, 0, 0, 1));
std::vector<float4> normals(knt.depth.size(), make_float4(0, 0, 1, 1));
std::vector<Eigen::Vector2f> grid_voxels_params(total_voxels);
//
// setup image parameters
//
unsigned short image_width = KINECT_V2_DEPTH_WIDTH;
unsigned short image_height = image_width / aspect_ratio;
QImage img(image_width, image_height, QImage::Format::Format_RGBA8888);
img.fill(Qt::GlobalColor::gray);
uchar4* image_data = (uchar4*)img.bits();
//float4* debug_buffer = new float4[image_width * image_height];
//memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
knt_cuda_setup(
vx_count, vx_size,
grid_matrix.data(),
projection.data(),
projection_inverse.data(),
*grid_voxels_params.data()->data(),
KINECT_V2_DEPTH_WIDTH,
KINECT_V2_DEPTH_HEIGHT,
KINECT_V2_DEPTH_MIN,
KINECT_V2_DEPTH_MAX,
vertices.data()[0],
normals.data()[0],
image_width,
image_height
);
timer.start();
knt_cuda_allocate();
knt_cuda_init_grid();
timer.print_interval("Allocating gpu : ");
timer.start();
knt_cuda_copy_host_to_device();
knt_cuda_copy_depth_buffer_to_device(knt.depth.data());
timer.print_interval("Copy host to device : ");
timer.start();
knt_cuda_normal_estimation();
timer.print_interval("Normal estimation : ");
timer.start();
knt_cuda_update_grid(view_matrix.data());
timer.print_interval("Update grid : ");
timer.start();
knt_cuda_grid_params_copy_device_to_host();
knt_cuda_copy_device_to_host();
timer.print_interval("Copy device to host : ");
//
// setup camera parameters
//
timer.start();
Eigen::Affine3f camera_to_world = Eigen::Affine3f::Identity();
float cam_z = -half_vol_size;
camera_to_world.scale(Eigen::Vector3f(1, 1, -1));
camera_to_world.translate(Eigen::Vector3f(half_vol_size, half_vol_size, cam_z));
Eigen::Matrix4f camera_to_world_matrix = camera_to_world.matrix();
knt_cuda_raycast(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, camera_to_world_matrix.data());
timer.print_interval("Raycast : ");
timer.start();
knt_cuda_copy_image_device_to_host(*(uchar4*)img.bits());
timer.print_interval("Copy Img to host : ");
timer.start();
knt_cuda_free();
timer.print_interval("Cleanup gpu : ");
#if 0
//memset(image_data, 0, image_width * image_height * sizeof(uchar4));
//memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
Eigen::Vector3f camera_pos = camera_to_world_matrix.col(3).head<3>();
float fov_scale = (float)tan(DegToRad(KINECT_V2_FOVY * 0.5f));
float aspect_ratio = KINECT_V2_DEPTH_ASPECT_RATIO;
//
// for each pixel, trace a ray
//
timer.start();
for (int y = 0; y < image_height; ++y)
{
for (int x = 0; x < image_width; ++x)
{
// Convert from image space (in pixels) to screen space
// Screen Space along X axis = [-aspect ratio, aspect ratio]
// Screen Space along Y axis = [-1, 1]
float x_norm = (2.f * float(x) + 0.5f) / (float)image_width;
float y_norm = (2.f * float(y) + 0.5f) / (float)image_height;
Eigen::Vector3f screen_coord(
(x_norm - 1.f) * aspect_ratio * fov_scale,
(1.f - y_norm) * fov_scale,
1.0f);
Eigen::Vector3f direction;
multDirMatrix(screen_coord, camera_to_world_matrix, direction);
direction.normalize();
long voxels_zero_crossing[2] = { -1, -1 };
int hit_count = raycast_tsdf_volume<float>(
camera_pos,
direction,
voxel_count.cast<int>(),
voxel_size.cast<int>(),
grid_voxels_params,
voxels_zero_crossing);
if (hit_count > 0)
{
if (hit_count == 2)
{
float4 n = normals[y * image_width + x];
//image_data[y * image_width + x].x = 0;
//image_data[y * image_width + x].y = 128;
//image_data[y * image_width + x].z = 128;
//image_data[y * image_width + x].w = 255;
image_data[y * image_width + x].x = uchar((n.x * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].y = uchar((n.y * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].z = uchar((n.z * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].w = 255;
}
else
{
image_data[y * image_width + x].x = 128;
image_data[y * image_width + x].y = 128;
image_data[y * image_width + x].z = 0;
image_data[y * image_width + x].w = 255;
}
}
else
{
image_data[y * image_width + x].x = 128;
image_data[y * image_width + x].y = 0;
image_data[y * image_width + x].z = 0;
image_data[y * image_width + x].w = 255;
}
}
}
timer.print_interval("Raycasting to image : ");
//export_debug_buffer("../../data/cpu_image_data_screen_coord_f4.txt", debug_buffer, image_width, image_height);
//export_image_buffer("../../data/cpu_image_data_screen_coord_uc.txt", image_data, image_width, image_height);
#else
//export_debug_buffer("../../data/gpu_image_data_screen_coord_f4.txt", debug_buffer, image_width, image_height);
//export_image_buffer("../../data/gpu_image_data_screen_coord_uc.txt", image_data, image_width, image_height);
#endif
QImage image(&image_data[0].x, image_width, image_height, QImage::Format_RGBA8888);
//image.fill(Qt::GlobalColor::black);
QApplication app(argc, argv);
QImageWidget widget;
widget.resize(KINECT_V2_DEPTH_WIDTH, KINECT_V2_DEPTH_HEIGHT);
widget.setImage(image);
widget.show();
return app.exec();
}
void ComputePnP(const char* folder_name,int start_idx,int total_num,int jump_num){//Compute two-view PnP
int img_idx = start_idx;
int img_num = total_num;
int img_num2 = jump_num;
char imgname[64];
char pcdname[64];
char imgname2[64];
char pcdname2[64];
ofstream out_pose,out_pose2;
out_pose.open("RT.txt");
out_pose2.open("invRT.txt");
cv::Mat colorImage_1(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_8UC4);
cv::Mat pointCloud_XYZforRGB_1(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_32FC3,cv::Scalar::all(0));
cv::Mat colorImage_2(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_8UC4);
cv::Mat pointCloud_XYZforRGB_2(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_32FC3,cv::Scalar::all(0));
cv::Mat img1,img2;
/////////////////////PCL objects//////////////////////////
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_1f (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_2f (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::visualization::PCLVisualizer vislm;
////////////////////Find cv::Matched points between two RGB images////////////////////////
ifstream cam_in("CamPara.txt");
cam_in>>CamIntrinsic[0][0]>>CamIntrinsic[0][1]>>CamIntrinsic[0][2]
>>CamIntrinsic[1][0]>>CamIntrinsic[1][1]>>CamIntrinsic[1][2]
>>CamIntrinsic[2][0]>>CamIntrinsic[2][1]>>CamIntrinsic[2][2];
cam_in.close();
const cv::Mat Camera_Matrix(3,3,CV_64F,CamIntrinsic);
const cv::Mat disCoef(1,5,CV_64F,DisCoef);
cv::Mat img_Matches;
int numKeyPoints = 400;
int numKeyPoints2 = 400;
RobustMatcher rMatcher;
cv::Ptr<cv::FeatureDetector> detector = new cv::FastFeatureDetector(0);
cv::Ptr<cv::FeatureDetector> detector2 = new cv::FastFeatureDetector(0);
cv::Ptr<cv::DescriptorExtractor> extractor = new cv::OrbDescriptorExtractor();
cv::Ptr<cv::DescriptorMatcher> Matcher= new cv::BFMatcher(cv::NORM_HAMMING);
rMatcher.setFeatureDetector(detector);
rMatcher.setDescriptorExtractor(extractor);
rMatcher.setDescriptorMatcher(Matcher);
std::vector<cv::KeyPoint> img1_keypoints;
cv::Mat img1_descriptors;
std::vector<cv::KeyPoint> img2_keypoints;
cv::Mat img2_descriptors;
std::vector<cv::DMatch > Matches;
//////////////////////PCL rigid motion estimation///////////////////////////
Eigen::Matrix4f TotaltransforMation = Eigen::Matrix4f::Identity();
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity();
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Transcloud_2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
//////////////////////////////////////////////////////////////////////////////
printf("From %d to %d",img_idx,img_idx+(img_num-1)*img_num2);
//Camera position
pcl::PointCloud<pcl::PointXYZ> Camera_pose0;
pcl::PointCloud<pcl::PointXYZ> Camera_pose;
Camera_pose0.push_back(pcl::PointXYZ(0,0,0));
Camera_pose0.push_back(pcl::PointXYZ(0.2,0,0));
Camera_pose0.push_back(pcl::PointXYZ(0,0.2,0));
Camera_pose0.push_back(pcl::PointXYZ(0,0,0.2));
/////////////Looping///////////////////////////
int d = 0;
for(int i=0; i<img_num-1; ++i, d+=img_num2){
sprintf(imgname,"%s/rgb_%d.jpg",folder_name,img_idx+d);
sprintf(pcdname,"%s/pcd_%d",folder_name,img_idx+d);
sprintf(imgname2,"%s/rgb_%d.jpg",folder_name,img_idx+d+img_num2);
sprintf(pcdname2,"%s/pcd_%d",folder_name,img_idx+d+img_num2);
printf("comparing %s & %s\n",imgname,imgname2);
////////////Reading data/////////////////////
img1 = cv::imread(imgname,CV_LOAD_IMAGE_GRAYSCALE);
colorImage_1 = cv::imread(imgname,CV_LOAD_IMAGE_COLOR);
printf("reading\n");
FILE* fin = fopen(pcdname,"rb");
fread(pointCloud_XYZforRGB_1.data,1,pointCloud_XYZforRGB_1.step*pointCloud_XYZforRGB_1.rows,fin);
fclose(fin);
img2 = cv::imread(imgname2,CV_LOAD_IMAGE_GRAYSCALE);
colorImage_2 = cv::imread(imgname2,CV_LOAD_IMAGE_COLOR);
printf("reading\n");
fin = fopen(pcdname2,"rb");
fread(pointCloud_XYZforRGB_2.data,1,pointCloud_XYZforRGB_2.step*pointCloud_XYZforRGB_2.rows,fin);
fclose(fin);
cloud_1->clear();
cloud_2->clear();
Mat2PCL_XYZRGB_ALL(colorImage_1,pointCloud_XYZforRGB_1,*cloud_1);
Mat2PCL_XYZRGB_ALL(colorImage_2,pointCloud_XYZforRGB_2,*cloud_2);
///////////////////Finding 2D features//////////////////////////
img1_keypoints.clear();
img2_keypoints.clear();
detector->detect(img1,img1_keypoints);
extractor->compute(img1,img1_keypoints,img1_descriptors);
detector2->detect(img2,img2_keypoints);
extractor->compute(img2,img2_keypoints,img2_descriptors);
Matches.clear();
printf("cv::Matching\n");
rMatcher.match(Matches, img1_keypoints, img2_keypoints,img1_descriptors,img2_descriptors);
//printf("drawing\n");
drawMatches(img1, img1_keypoints, img2, img2_keypoints,Matches, img_Matches, cv::Scalar::all(-1), cv::Scalar::all(-1),vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
cv::imshow("Good matches",img_Matches);
cv::waitKey(10);
///////////////////Find corresponding 3D pints////////////////////////////////////////
//We should draw point cloud from the Matches...
//////////////////3D geometry///////////////////////////////////////////////////////
/////////////////////////
//Compute PnP
////////////////////////
cv::Mat rvec(1,3,cv::DataType<double>::type);
cv::Mat tvec(1,3,cv::DataType<double>::type);
cv::Mat rotationMatrix(3,3,cv::DataType<double>::type);
vector<cv::Point2f> p2ds;
vector<cv::Point3f> p3ds;
Mat2PCL_XYZRGB_MATCH_PnP(pointCloud_XYZforRGB_1,p3ds,p2ds,img1_keypoints,img2_keypoints,Matches);
printf("3D/2D points:%d,%d\n",p3ds.size(),p2ds.size());
if(p3ds.size() > 5){
cv::solvePnPRansac(p3ds,p2ds,Camera_Matrix,disCoef,rvec,tvec);
cv::Rodrigues(rvec,rotationMatrix);
PnPret2Mat4f(rotationMatrix,tvec,Ti);
Ti = Ti.inverse();
}
///////////////////////Compose the translation, and transform the point cloud////////////////
Transcloud_2->clear();
std::cout<<"\nLocal motion from PnP is \n"<<Ti;
TotaltransforMation = TotaltransforMation*Ti;
pcl::transformPointCloud(*cloud_2,*Transcloud_2,TotaltransforMation);
std::cout<<"\nTotal motion from PnP is \n"<<TotaltransforMation<<endl;
//Add camera coordinate
pcl::transformPointCloud(Camera_pose0,Camera_pose,TotaltransforMation);
sprintf(pcdname2,"X%d",img_idx+d+img_num2);
vislm.addLine(Camera_pose[0],Camera_pose[1],255,0,0,pcdname2);
sprintf(pcdname2,"Y%d",img_idx+d+img_num2);
vislm.addLine(Camera_pose[0],Camera_pose[2],0,255,0,pcdname2);
sprintf(pcdname2,"Z%d",img_idx+d+img_num2);
vislm.addLine(Camera_pose[0],Camera_pose[3],0,0,255,pcdname2);
if(i==0){
vislm.addPointCloud(cloud_1->makeShared(),pcdname);
//PCLviewer.showCloud(cloud_1,"init");
out_pose<<Eigen::Matrix4f::Identity()<<endl;
}
vislm.addPointCloud(Transcloud_2->makeShared(),pcdname2);
out_pose<<TotaltransforMation<<endl;
out_pose2<<TotaltransforMation.inverse()<<endl;
}
vislm.resetCamera();
vislm.spin();
out_pose.close();
out_pose2.close();
}
