int main (int argc, char** argv)
{
if (argc != 3)
{
ROS_INFO_STREAM("please provide a pointcloud file followed by a text file containing a transformation matrix as arguments");
exit(0);
}
pcl::PCDReader reader;
pcl::PointCloud<PointType>::Ptr cloudIn (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr cloudOut (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr cloudOut_inv (new pcl::PointCloud<PointType>);
Eigen::Matrix4f trafo, trafo_inv;
reader.read (argv[1], *cloudIn);
std::ifstream myfile;
myfile.open (argv[2]);
for (int row = 0; row < 4; row++)
for (int col = 0; col < 4; col++)
{
myfile >> trafo (row, col);
}
trafo_inv = trafo.inverse();
ROS_INFO_STREAM("transform to be used: \n" << trafo);
transformPointCloud (*cloudIn, *cloudOut, trafo);
transformPointCloud (*cloudIn, *cloudOut_inv, trafo_inv);
pcl::PCDWriter writer;
writer.write ("output.pcd", *cloudOut, false);
writer.write ("output_inverse.pcd", *cloudOut_inv, false);
return (0);
}
template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output)
{
if (!input_features_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
return;
}
if (!target_features_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
return;
}
std::vector<int> sample_indices (nr_samples_);
std::vector<int> corresponding_indices (nr_samples_);
PointCloudSource input_transformed;
float error, lowest_error (0);
final_transformation_ = Eigen::Matrix4f::Identity ();
for (int i_iter = 0; 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, (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_);
}
/**
* mergeFrames function
*
* This function merges multiple point clouds to one point cloud
* @param std::vector<Frame3D> frames
* @param std::string filename the filename to save to
* @return point cloud
*/
pcl::PointCloud<pcl::PointNormal>::Ptr Functions3D::mergeFrames(const std::vector<Frame3D> &frames, std::string filename)
{
/**
* Initialize a new point cloud
*/
pcl::PointCloud<pcl::PointNormal>::Ptr total_cloud(new pcl::PointCloud<pcl::PointNormal>);
/**
* Check if the file exists, otherwise process all frames
*/
if (pcl::io::loadPLYFile<pcl::PointNormal>("../data/" + filename, *total_cloud) == -1)
{
/**
* Loop over all frames in the vector
*/
for (size_t i = 0; i < frames.size(); i++)
{
std::cout << "Processing frame " << i << std::endl;
// Save reference instead of copy
const Frame3D ¤t_frame = frames[i];
// get data from the frame
cv::Mat depth = current_frame.depth_image_;
double focal = current_frame.focal_length_;
const Eigen::Matrix4f camera_pose = current_frame.getEigenTransform();
// compute cloud nessecary to append the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud = depthToPointCloud(depth, focal, 1);
pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud = computeNormals(pcloud);
pcl::PointCloud<pcl::PointNormal>::Ptr transformed_normal_cloud = transformPointCloud(normal_cloud, camera_pose);
// add the cloud to the existing point cloud
addPointCloud(total_cloud, transformed_normal_cloud);
}
// @todo the combination of this with the load function does not work yet
pcl::io::savePLYFile("../data/" + filename, *total_cloud);
}
/**
* Convert to XYZ to show the cloud
* uncomment other code to show the point cloud
*/
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// pcl::copyPointCloud(*total_cloud, *cloud);
// pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
// viewer.showCloud(cloud);
// while (!viewer.wasStopped()) {}
/**
* Return the concatenated clouds
*/
return total_cloud;
}
template <typename PointT, typename Scalar> inline void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
const Eigen::Quaternion<Scalar> &rotation)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
transformPointCloud (cloud_in, cloud_out, t);
}
template <typename PointT> inline void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Vector3f &offset,
const Eigen::Quaternionf &rotation)
{
Eigen::Translation3f translation (offset);
// Assemble an Eigen Transform
Eigen::Affine3f t;
t = translation * rotation;
transformPointCloud (cloud_in, cloud_out, t);
}
void visualization::DetectionVisualizer::visualizeSampledGrasps()
{
if (obj().draw_sampled_grasps_)
{
Point middle;
middle.getVector3fMap() = obj().obj_bounding_box_->position_base_kinect_frame_;
removeShape(SUPPORT_PLANE);
addPlane(*(obj().table_plane_), middle.x, middle.y, middle.z, SUPPORT_PLANE);
CloudPtr side_grasps = obj().getSampledSideGrasps();
const pcl::PCLHeader &header = obj().world_obj_->header;
transformPointCloud(FOOTPRINT_FRAME, header.frame_id, side_grasps, side_grasps,header.stamp,tf_listener_);
visualizeCloud(SIDE_GRASPS, side_grasps, 255, 0, 0);
CloudPtr top_grasps = obj().getSampledTopGrasps();
transformPointCloud(FOOTPRINT_FRAME, header.frame_id, top_grasps, top_grasps,header.stamp,tf_listener_);
visualizeCloud(TOP_GRASPS, top_grasps, 255, 0, 0);
//visualizePoint(middle, 0, 0, 255, CENTROID);
obj().draw_sampled_grasps_ = false;
}
}
Pointcloud::Ptr joint(camera &cam, Pointcloud::Ptr points, Isometry3d &T, frame &newframe)
{
Pointcloud::Ptr cloud = map2point(cam, newframe.rgb, newframe.depth);
cout << "combining clouds together" << endl;
Pointcloud::Ptr output(new Pointcloud());
//transform cloud points into the same coordinate system
transformPointCloud(*points, *output, T.matrix());
//put the points together
*output += *cloud;
return output;
}
void ICP::run(bool withCuda, InputArray initObjSet)
{
assert(!m_objSet.empty() && !m_modSet.empty());
double d_pre = 100000, d_now = 100000;
int iterCnt = 0;
Mat objSet;
Transformation tr;
if (initObjSet.empty())
{
objSet = m_objSet.clone();
}
else
{
objSet = initObjSet.getMat();
}
/* plotTwoPoint3DSet(objSet, m_modSet);*/
do
{
d_pre = d_now;
Mat closestSet;
Mat lambda(objSet.rows, 1, CV_64FC1);
RUNANDTIME(global_timer, closestSet =
getClosestPointsSet(objSet, lambda, KDTREE).clone(),
OUTPUT && SUBOUTPUT, "compute closest points.");
Mat tmpObjSet = convertMat(m_objSet);
Mat tmpModSet = convertMat(closestSet);
RUNANDTIME(global_timer, tr =
computeTransformation(tmpObjSet, tmpModSet, lambda),
OUTPUT && SUBOUTPUT, "compute transformation");
Mat transformMat = tr.toMatrix();
RUNANDTIME(global_timer, transformPointCloud(
m_objSet, objSet, transformMat, withCuda),
OUTPUT && SUBOUTPUT, "transform points.");
RUNANDTIME(global_timer,
d_now = computeError(objSet, closestSet, lambda, withCuda),
OUTPUT && SUBOUTPUT, "compute error.");
iterCnt++;
} while (fabs(d_pre - d_now) > m_epsilon && iterCnt <= m_iterMax);
m_tr = tr;
/* waitKey();*/
/* plotTwoPoint3DSet(objSet, m_modSet);*/
}
template <typename PointSource, typename PointTarget> double
pcl16::registration::TransformationValidationEuclidean<PointSource, PointTarget>::validateTransformation (
const PointCloudSourceConstPtr &cloud_src,
const PointCloudTargetConstPtr &cloud_tgt,
const Eigen::Matrix4f &transformation_matrix)
{
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
pcl16::PointCloud<PointSource> input_transformed;
transformPointCloud (*cloud_src, input_transformed, transformation_matrix);
// Just in case
if (!tree_)
tree_.reset (new pcl16::KdTreeFLANN<PointTarget>);
tree_->setInputCloud (cloud_tgt);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < input_transformed.points.size (); ++i)
{
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > max_range_)
continue;
// Optimization: use getVector4fMap instead, but make sure that the last coordinate is 0!
Eigen::Vector4f p1 (input_transformed.points[i].x,
input_transformed.points[i].y,
input_transformed.points[i].z, 0);
Eigen::Vector4f p2 (cloud_tgt->points[nn_indices[0]].x,
cloud_tgt->points[nn_indices[0]].y,
cloud_tgt->points[nn_indices[0]].z, 0);
// Calculate the fitness score
fitness_score += fabs ((p1-p2).squaredNorm ());
nr++;
}
if (nr > 0)
return (fitness_score / nr);
else
return (std::numeric_limits<double>::max ());
}
/* return value is the best match index */
int ModelMaker::computeAllGradientEnergys(){
Vec6f paraCurrent(mtYaw, mtPitch, mtRoll, mtTransX, mtTransY, mtTransZ);
Vec6f paraPeek;
int max = INT_MIN;
float min = FLT_MAX;
int idx = -1;
int inlier;
float dist;
for(int i = 0 ; i < 13 ; i ++){
if(i > 6)
for(int j = 0 ; j < 6 ; j++) paraPeek[j] = paraCurrent[j] + gradient[i][j] * mtTStep;
else if(i > 0)
for(int j = 0 ; j < 6 ; j++) paraPeek[j] = paraCurrent[j] + gradient[i][j] * mtRStep;
else
for(int j = 0 ; j < 6 ; j++) paraPeek[j] = paraCurrent[j];
transformPointCloud(paraPeek, this->srcPointCloud, this->srcCenter, this->bufCloud, this->bufCenter);
//printf("(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f)", paraPeek[0], paraPeek[1], paraPeek[2], paraPeek[3], paraPeek[4], paraPeek[5]);
dist = errFuncDist(this->bufCloud, 20);
if( dist < min ){
min = dist;
idx = i;
}
/* use inliner count for energy function
inlier = errFuncInlier(this->bufCloud, 20);
if( inlier > max ){
max = inlier;
idx = i;
}
*/
}
//printf("= = = = MAX = = = =\n");
float step;
step=(idx>6)?mtTStep:mtRStep;
mtYaw += gradient[idx][0] * step;
mtPitch += gradient[idx][1] * step;
mtRoll += gradient[idx][2] * step;
mtTransX += gradient[idx][3] * step;
mtTransY += gradient[idx][4] * step;
mtTransZ += gradient[idx][5] * step;
//printf("(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f) : %d\n", mtYaw, mtPitch, mtRoll, mtTransX, mtTransY, mtTransZ, max);
printf("(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f) : %.2f ", mtYaw, mtPitch, mtRoll, mtTransX, mtTransY, mtTransZ, min);
return idx;
}
void
transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform,
const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
{
if (in.header.frame_id == target_frame)
{
out = in;
return;
}
// Get the transformation
Eigen::Matrix4f transform;
transformAsMatrix (net_transform, transform);
transformPointCloud (transform, in, out);
out.header.frame_id = target_frame;
}
bool SensorProcessorBase::process(
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pointCloudInput,
const Eigen::Matrix<double, 6, 6>& robotPoseCovariance,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudMapFrame,
Eigen::VectorXf& variances)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudSensorFrame(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pointCloudInput, *pointCloudSensorFrame);
cleanPointCloud(pointCloudSensorFrame);
ros::Time timeStamp;
timeStamp.fromNSec(1000 * pointCloudSensorFrame->header.stamp);
if (!updateTransformations(pointCloudSensorFrame->header.frame_id, timeStamp)) return false;
if (!transformPointCloud(pointCloudSensorFrame, pointCloudMapFrame, mapFrameId_)) return false;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> pointClouds({pointCloudMapFrame, pointCloudSensorFrame});
removePointsOutsideLimits(pointCloudMapFrame, pointClouds);
if (!computeVariances(pointCloudSensorFrame, robotPoseCovariance, variances)) return false;
return true;
}
void visualization::DetectionVisualizer::visualizeBoundingBox()
{
if (obj().draw_bounding_box_)
{
detection::BoundingBox &box = *(obj().obj_bounding_box_);
// Draw world coordinate system
removeCoordinateSystem();
addCoordinateSystem(0.5);
addCoordinateSystem(0.25, box.getObjToKinectTransform());
CloudPtr &obj_kinect_frame = obj().world_obj_;
visualizeCloud(OBJ_KINECT_FRAME, obj_kinect_frame, 0, 255, 0);
visualizeCloud(OBJ_2D, obj().planar_obj_, 120, 120, 120);
CloudPtr obj_frame(new Cloud);
transformPointCloud(obj_kinect_frame->header.frame_id, box.OBJ_FRAME, obj_kinect_frame, obj_frame,
obj_kinect_frame->header.stamp, tf_listener_);
visualizeCloud(OBJ_3D, obj_frame, 0, 0, 255);
visualizeCloud(WORLD_PLANAR_OBJ, box.obj_2D_kinect_frame, 0, 0, 255);
// Draw the box in world coords
visualizeBox(box, WORLD_BOUNDING_BOX, box.position_3D_kinect_frame_, box.getRotationQuaternion());
// Draw the box in the origin (obj coords)
visualizeBox(box, BOUNDING_BOX);
CloudPtr a(new Cloud);
a->push_back(newPoint(box.position_base_kinect_frame_));
a->push_back(newPoint(box.position_3D_kinect_frame_));
visualizeCloud("a", a, 0, 0, 255);
CloudPtr b(new Cloud);
b->push_back(newPoint(box.centroid_2D_kinect_frame_.head<3>()));
b->push_back(newPoint(box.planar_shift_));
//b->push_back(newPoint(box.));
visualizeCloud("b", b, 255, 0, 0);
//showEigenVectors(box);
obj().draw_bounding_box_ = false;
}
}
template <typename PointSource, typename PointTarget> inline double
pcl::Registration<PointSource, PointTarget>::getFitnessScore (double max_range)
{
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
PointCloudSource input_transformed;
transformPointCloud (*input_, input_transformed, final_transformation_);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < input_transformed.points.size (); ++i)
{
Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.points[i].x,
input_transformed.points[i].y,
input_transformed.points[i].z, 0);
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] > max_range)
continue;
Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x,
target_->points[nn_indices[0]].y,
target_->points[nn_indices[0]].z, 0);
// Calculate the fitness score
fitness_score += fabs ((p1-p2).squaredNorm ());
nr++;
}
if (nr > 0)
return (fitness_score / nr);
else
return (std::numeric_limits<double>::max ());
}
int TYCloudMatcher::bestMatchedFE(cv::Vec6f &pose, Vec3f &vecOM, const std::vector<cv::Point3f> &pCloud, const cv::Point3f &nose){
float min = FLT_MAX;
float dist;
int idx = -1;
argCurrent = pose;
argCurrent[3] += vecOM[0];
argCurrent[4] += vecOM[1];
argCurrent[5] += vecOM[2];
/* Translate and rotate the source point cloud by the current argument */
transformPointCloud(argCurrent, pCloud, nose, this->bufCloud, this->bufCenter);
printf("kdtree size = %d, (",kdtree.size());
for(unsigned int i = 0 ; i < kdtree.size() ; i++){
/* Calculate Energy(Distance) */
dist = energy(this->bufCloud, i);
printf("%.2f, ",dist);
/* Keep the best one */
if( dist < min ){
min = dist;
idx = i;
}
}
printf(") ");
argCurrent[3] -= vecOM[0];
argCurrent[4] -= vecOM[1];
argCurrent[5] -= vecOM[2];
return idx;
};
void moveModelToOrigin (const PointCloudConstPtr cloud_input_ptr,
const PointCloudPtr cloud_output_ptr,
Eigen::Matrix4f& transform_output)
{
//find min in x,y and z. Move this to Origin
float min_x, min_y, min_z;
min_x = min_y = min_z = std::numeric_limits<float>::max ();
for (std::vector<PointType, Eigen::aligned_allocator<PointType> >::const_iterator itr =
cloud_input_ptr->points.begin ();
itr != cloud_input_ptr->points.end (); itr++)
{
if (itr->x < min_x)
min_x = itr->x;
if (itr->y < min_y)
min_y = itr->y;
if (itr->z < min_z)
min_z = itr->z;
}
transform_output = Eigen::Matrix4f::Identity (4, 4);
transform_output (0, 3) = -min_x;
transform_output (1, 3) = -min_y;
transform_output (2, 3) = -min_z;
transformPointCloud (*cloud_input_ptr, *cloud_output_ptr, transform_output);
}
bool
transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in,
sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
{
if (in.header.frame_id == target_frame)
{
out = in;
return (true);
}
// Get the TF transform
tf::StampedTransform transform;
try
{
tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
}
catch (tf::LookupException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
catch (tf::ExtrapolationException &e)
{
ROS_ERROR ("%s", e.what ());
return (false);
}
// Convert the TF transform to Eigen format
Eigen::Matrix4f eigen_transform;
transformAsMatrix (transform, eigen_transform);
transformPointCloud (eigen_transform, in, out);
out.header.frame_id = target_frame;
return (true);
}
void FloorAxisAlignment::alignCloudPrincipleAxis (const PointCloudConstPtr input_cloud_ptr,
const PointCloudPtr output_cloud_ptr,
Eigen::Matrix4f& transform_output)
{
// Visualization visualizer;
Eigen::Matrix4f inital_guess;
inital_guess.block<3,3>(0,0) = Eigen::AngleAxisf(angle_, axis_).toRotationMatrix();
// inital_guess = Eigen::Matrix4f::Zero(4,4);
// inital_guess(0,0) = 1.0;
// inital_guess(1,2) = 1.0;
// inital_guess(2,1) = -1.0;
// inital_guess(3,3) = 1.0;
PointCloudPtr rotated_cloud_ptr (new PointCloud);
transformPointCloud (*input_cloud_ptr, *rotated_cloud_ptr, inital_guess);
// std::vector<PointCloudConstPtr> vis_cloud_ptrs;
// vis_cloud_ptrs.push_back(rotated_cloud_ptr);
//transformPointCloud (*input_cloud_ptr, *output_cloud_ptr, inital_guess);
//assume the model is approx correct, i.e. z is height
// find the largest plane perpendicular to z axis and use this to align cloud
pcl17::ModelCoefficients::Ptr coefficients (new pcl17::ModelCoefficients);
pcl17::PointIndices::Ptr inliers (new pcl17::PointIndices);
pcl17::SACSegmentation<PointType> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl17::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl17::SAC_RANSAC);
seg.setDistanceThreshold (ransac_threshold_);
seg.setMaxIterations(max_iterations_);
//seg.setProbability(0.1);
seg.setAxis (Eigen::Vector3f (0, 0, 1));
seg.setEpsAngle (30.0 * (M_PI / 180));//radians
seg.setInputCloud (rotated_cloud_ptr);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL17_ERROR("Could not estimate a planar model for the given dataset.");
// I had some issues with pcl on a different computer to where I normally dev. This is a workaround to be removed
coefficients->values.push_back(-0.0370391);
coefficients->values.push_back(0.0777064);
coefficients->values.push_back(0.996288);
coefficients->values.push_back(2.63374);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1]
<< " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
// calculate angle and axis from dot product
double rotate = acos (coefficients->values[2]);
Eigen::Vector3f axis_to_rotate_about (-coefficients->values[1], coefficients->values[0], 0.0);
axis_to_rotate_about.normalize ();
//convert axis and angle to rotation matrix and apply to pointcloud
Eigen::AngleAxis<float> angle_axis (rotate, axis_to_rotate_about);
transform_output = Eigen::Matrix4f::Identity (4, 4);
transform_output.block<3, 3> (0, 0) = angle_axis.toRotationMatrix ().inverse ();
// apply translation to bring the floor to z = 0
transform_output(2, 3) = coefficients->values[3];
transformPointCloud (*rotated_cloud_ptr, *output_cloud_ptr, transform_output);
// pcl17::ExtractIndices<PointType> eifilter;
// eifilter.setInputCloud (rotated_cloud_ptr);
// eifilter.setIndices (inliers);
// eifilter.filter (*output_cloud_ptr);
// vis_cloud_ptrs.push_back(output_cloud_ptr);
// visualizer.visualizeCloud(vis_cloud_ptrs);
}
void VoxelMapProvider::init(Provider_Parameter& parameter)
{
m_mutex.lock();
const string prefix = "VoxelMapProvider::" + string(__FUNCTION__);
const string temp_timer = prefix + "_temp";
PERF_MON_START(prefix);
PERF_MON_START(temp_timer);
Provider::init(parameter);
// get shared memory pointer
m_shm_memHandle = m_segment.find_or_construct<cudaIpcMemHandle_t>(
std::string(shm_variable_name_voxelmap_handler_dev_pointer + m_shared_mem_id).c_str())(
cudaIpcMemHandle_t());
m_shm_mapDim = m_segment.find_or_construct<Vector3ui>(
std::string(shm_variable_name_voxelmap_dimension + m_shared_mem_id).c_str())(Vector3ui(0));
m_shm_VoxelSize = m_segment.find_or_construct<float>(
std::string(shm_variable_name_voxel_side_length + m_shared_mem_id).c_str())(0.0f);
// there should only be one segment of number_of_voxelmaps
std::pair<uint32_t*, std::size_t> r = m_segment.find<uint32_t>(
shm_variable_name_number_of_voxelmaps.c_str());
if (r.second == 0)
{
// if it doesn't exist ..
m_segment.construct<uint32_t>(shm_variable_name_number_of_voxelmaps.c_str())(1);
}
else
{
// if it exit increase it by one
(*r.first)++;
}
Vector3ui map_dim;
std::vector<Vector3f> insert_points;
float voxel_map_res = 1.0f;
if (parameter.points.size() != 0)
{
Vector3f offset;
Vector3ui point_data_bounds = getMapDimensions(parameter.points, offset);
map_dim = point_data_bounds;
printf("point cloud dimension %u %u %u\n", map_dim.x, map_dim.y, map_dim.z);
if (parameter.plan_size.x != 0.0f && parameter.plan_size.y != 0.0f && parameter.plan_size.z != 0.0f)
{
Vector3f tmp = parameter.plan_size * 1000.0f;
map_dim = Vector3ui(uint32_t(tmp.x), uint32_t(tmp.y), uint32_t(tmp.z));
printf("dim in cm %u %u %u\n", map_dim.x, map_dim.y, map_dim.z);
}
uint64_t map_voxel = uint64_t(map_dim.x) * uint64_t(map_dim.y) * uint64_t(map_dim.z);
float scaling = 1.0;
if (parameter.max_memory == 0)
{
// compute scaling factor based on voxel size
scaling = 1.0f / parameter.resolution_tree;
}
else
{
// compute max scaling factor based on memory restriction
uint64_t max_voxel = uint64_t(parameter.max_memory) / sizeof(ProbabilisticVoxel);
printf("max_voxel %lu map_voxel %lu\n", max_voxel, map_voxel);
if (max_voxel <= map_voxel)
scaling = float(pow(max_voxel / double(map_voxel), 1.0 / 3));
}
printf("scaling %f\n", scaling);
std::vector<Vector3ui> points;
Vector3ui map_dim_tmp;
transformPointCloud(parameter.points, points, map_dim_tmp, scaling * 1000.0f);
map_dim = Vector3ui(uint32_t(ceil(map_dim.x * scaling)), uint32_t(ceil(map_dim.y * scaling)),
uint32_t(ceil(map_dim.z * scaling)));
printf("voxel map dimension %u %u %u\n", map_dim.x, map_dim.y, map_dim.z);
// center data at the middle of the map, just like for NTree
point_data_bounds = Vector3ui(uint32_t(ceil(point_data_bounds.x * scaling)),
uint32_t(ceil(point_data_bounds.y * scaling)),
uint32_t(ceil(point_data_bounds.z * scaling)));
Vector3ui tmp_offset = (map_dim - point_data_bounds) / Vector3ui(2);
insert_points.resize(points.size());
printf("scaling %f\n", scaling);
voxel_map_res = (1.0f / scaling) / 1000.0f;;
printf("mapres %f\n", voxel_map_res);
for (int i = 0; i < int(points.size()); ++i)
{
points[i] = points[i] + tmp_offset;
insert_points[i].x = points[i].x * voxel_map_res + voxel_map_res / 2;
insert_points[i].y = points[i].y * voxel_map_res + voxel_map_res / 2;
insert_points[i].z = points[i].z * voxel_map_res + voxel_map_res / 2;
}
PERF_MON_START(temp_timer);
}
else
{
// VoxelMap with same size as octree
uint32_t dim = (uint32_t) pow(pow(BRANCHING_FACTOR, 1.0 / 3), parameter.resolution_tree);
map_dim = Vector3ui(dim);
voxel_map_res = parameter.resolution_tree * 0.001f; // voxel size in meter
}
switch(m_parameter->model_type)
{
case Provider_Parameter::eMT_Probabilistic:
{
m_voxelMap = new gpu_voxels::voxelmap::ProbVoxelMap(map_dim.x, map_dim.y, map_dim.z, voxel_map_res, MT_PROBAB_VOXELMAP);
break;
}
case Provider_Parameter::eMT_BitVector:
{
m_voxelMap = new gpu_voxels::voxelmap::BitVectorVoxelMap(map_dim.x, map_dim.y, map_dim.z, voxel_map_res, MT_BITVECTOR_VOXELMAP);
break;
}
default:
{
printf("ERROR: Unknown 'model_type'\n");
}
}
m_segment.find_or_construct<MapType>(std::string(shm_variable_name_voxelmap_type + m_shared_mem_id).c_str())(m_voxelMap->getMapType());
if (insert_points.size() != 0)
{
m_voxelMap->insertPointCloud(insert_points, gpu_voxels::eBVM_OCCUPIED);
// if (m_parameter->model_type == Provider_Parameter::eMT_BitVector)
// {
// Vector3f offset(1, 1, 1);
// for (uint32_t k = 1; k < 4; ++k)
// {
// std::vector<Vector3f> tmp = insert_points;
// for (int i = 0; i < int(tmp.size()); ++i)
// tmp[i] = tmp[i] + offset * k;
//
// m_voxelMap->insertPointCloud(tmp, gpu_voxels::eBVM_UNDEFINED + k);
// }
// }
PERF_MON_PRINT_INFO_P(temp_timer, "Build", prefix);
}
PERF_MON_ADD_DATA_NONTIME_P("UsedMemory", m_voxelMap->getMemoryUsage(), prefix);
m_sensor_orientation = gpu_voxels::Vector3f(0, 0, 0);
m_sensor_position = gpu_voxels::Vector3f(
(m_voxelMap->getDimensions().x * m_voxelMap->getVoxelSideLength()) / 2,
(m_voxelMap->getDimensions().y * m_voxelMap->getVoxelSideLength()) / 2,
(m_voxelMap->getDimensions().z * m_voxelMap->getVoxelSideLength()) / 2) * 0.001f; // in meter
printf("VoxelMap created!\n");
m_mutex.unlock();
}
template<typename PointSource, typename PointTarget> void
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
nr_iterations_ = 0;
converged_ = false;
double gauss_c1, gauss_c2, gauss_d3;
// Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
gauss_c1 = 10 * (1 - outlier_ratio_);
gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
gauss_d3 = -log (gauss_c2);
gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3;
gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Initialize Point Gradient and Hessian
point_gradient_.setZero ();
point_gradient_.block<3, 3>(0, 0).setIdentity ();
point_hessian_.setZero ();
Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
eig_transformation.matrix () = final_transformation_;
// Convert initial guess matrix to 6 element transformation vector
Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
Eigen::Vector3f init_translation = eig_transformation.translation ();
Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
p << init_translation (0), init_translation (1), init_translation (2),
init_rotation (0), init_rotation (1), init_rotation (2);
Eigen::Matrix<double, 6, 6> hessian;
double score = 0;
double delta_p_norm;
// Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination.
score = computeDerivatives (score_gradient, hessian, output, p);
while (!converged_)
{
// Store previous transformation
previous_transformation_ = transformation_;
// Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Negative for maximization as opposed to minimization
delta_p = sv.solve (-score_gradient);
//Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
delta_p_norm = delta_p.norm ();
if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
{
trans_probability_ = score / static_cast<double> (input_->points.size ());
converged_ = delta_p_norm == delta_p_norm;
return;
}
delta_p.normalize ();
delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
delta_p *= delta_p_norm;
transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
p = p + delta_p;
// Update Visualizer (untested)
if (update_visualizer_ != 0)
update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
if (nr_iterations_ > max_iterations_ ||
(nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
{
converged_ = true;
}
nr_iterations_++;
}
// Store transformation probability. The realtive differences within each scan registration are accurate
// but the normalization constants need to be modified for it to be globally accurate
trans_probability_ = score / static_cast<double> (input_->points.size ());
}
template<typename PointSource, typename PointTarget> double
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
PointCloudSource &trans_cloud)
{
// Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
double phi_0 = -score;
// Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
double d_phi_0 = -(score_gradient.dot (step_dir));
Eigen::Matrix<double, 6, 1> x_t;
if (d_phi_0 >= 0)
{
// Not a decent direction
if (d_phi_0 == 0)
return 0;
else
{
// Reverse step direction and calculate optimal step.
d_phi_0 *= -1;
step_dir *= -1;
}
}
// The Search Algorithm for T(mu) [More, Thuente 1994]
int max_step_iterations = 10;
int step_iterations = 0;
// Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
double mu = 1.e-4;
// Curvature condition constant, Equation 1.2 [More, Thuete 1994]
double nu = 0.9;
// Initial endpoints of Interval I,
double a_l = 0, a_u = 0;
// Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
// Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
bool interval_converged = (step_max - step_min) > 0, open_interval = true;
double a_t = step_init;
a_t = std::min (a_t, step_max);
a_t = std::max (a_t, step_min);
x_t = x + step_dir * a_t;
final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
// New transformed point cloud
transformPointCloud (*input_, trans_cloud, final_transformation_);
// Updates score, gradient and hessian. Hessian calculation is unessisary but testing showed that most step calculations use the
// initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
// Calculate phi(alpha_t)
double phi_t = -score;
// Calculate phi'(alpha_t)
double d_phi_t = -(score_gradient.dot (step_dir));
// Calculate psi(alpha_t)
double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t)
double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
// Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
{
// Use auxilary function if interval I is not closed
if (open_interval)
{
a_t = trialValueSelectionMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, psi_t, d_psi_t);
}
else
{
a_t = trialValueSelectionMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, phi_t, d_phi_t);
}
a_t = std::min (a_t, step_max);
a_t = std::max (a_t, step_min);
x_t = x + step_dir * a_t;
final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
// New transformed point cloud
// Done on final cloud to prevent wasted computation
transformPointCloud (*input_, trans_cloud, final_transformation_);
// Updates score, gradient. Values stored to prevent wasted computation.
score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);
// Calculate phi(alpha_t+)
phi_t = -score;
// Calculate phi'(alpha_t+)
d_phi_t = -(score_gradient.dot (step_dir));
// Calculate psi(alpha_t+)
psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t+)
d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
// Check if I is now a closed interval
if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
{
open_interval = false;
// Converts f_l and g_l from psi to phi
f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
g_l = g_l + mu * d_phi_0;
// Converts f_u and g_u from psi to phi
f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
g_u = g_u + mu * d_phi_0;
}
if (open_interval)
{
// Update interval end points using Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, psi_t, d_psi_t);
}
else
{
// Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT (a_l, f_l, g_l,
a_u, f_u, g_u,
a_t, phi_t, d_phi_t);
}
step_iterations++;
}
// If inner loop was run then hessian needs to be calculated.
// Hessian is unnessisary for step length determination but gradients are required
// so derivative and transform data is stored for the next iteration.
if (step_iterations)
computeHessian (hessian, trans_cloud, x_t);
return (a_t);
}
void getTestDataFromBag(PointCloudPtr cloud_source, PointCloudPtr cloud_target,
PointCloudPtr featureCloudSource, std::vector<int> &indicesSource,
PointCloudPtr featureCloudTarget, std::vector<int> &indicesTarget,
Eigen::Matrix4f &initialTransform, int rosMessageNumber)
{
sensor_msgs::PointCloud2::Ptr cloud_source_filtered (new sensor_msgs::PointCloud2 ());
sensor_msgs::PointCloud2::Ptr cloud_target_filtered (new sensor_msgs::PointCloud2 ());
PointCloudPtr cloud_ransac_estimation (new PointCloud);
pcl::PCDWriter writer;
std::stringstream filename;
rosbag::Bag bag;
bag.open("/media/burg/data/bagfiles/bench1-ManySweeps4-lowfeatureThresNode2.bag", rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("/feature_match_out_topic"));
rosbag::View view(bag, rosbag::TopicQuery(topics));
int i = 1;
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
if(( i == rosMessageNumber) || (rosMessageNumber==0))
{
pcl_tutorial::featureMatch::ConstPtr fm = m.instantiate<pcl_tutorial::featureMatch>();
ROS_INFO("Converting point cloud message to local pcl clouds");
sensor_msgs::PointCloud2::Ptr cloud_source_temp_Ptr (new sensor_msgs::PointCloud2 (fm->sourcePointcloud));
sensor_msgs::PointCloud2::Ptr cloud_target_temp_Ptr (new sensor_msgs::PointCloud2 (fm->targetPointcloud));
voxFilterPointCloud(cloud_source_temp_Ptr, cloud_source_filtered);
voxFilterPointCloud(cloud_target_temp_Ptr, cloud_target_filtered);
ROS_INFO("Converting dense clouds");
pcl::fromROSMsg(*cloud_source_filtered, *cloud_source);
pcl::fromROSMsg(*cloud_target_filtered, *cloud_target);
ROS_INFO("Converting sparse clouds");
pcl::fromROSMsg(fm->sourceFeatureLocations, *featureCloudSource);
pcl::fromROSMsg(fm->targetFeatureLocations, *featureCloudTarget);
ROS_INFO("Converting geometry message to eigen4f");
tf::Transform trans;
tf::transformMsgToTF(fm->featureTransform,trans);
initialTransform = EigenfromTf(trans);
ROS_INFO_STREAM("transform from ransac: " << "\n" << initialTransform << "\n");
ROS_INFO("Extracting corresponding indices");
//int j = 1;
indicesSource.clear();
indicesTarget.clear();
for(std::vector<pcl_tutorial::match>::const_iterator iterator_ = fm->matches.begin(); iterator_ != fm->matches.end(); ++iterator_)
{
indicesSource.push_back(iterator_->trainId);
indicesTarget.push_back(iterator_->queryId);
// ROS_INFO_STREAM("source point " << j << ": " << featureCloudSource->points[iterator_->queryId].x << ", " << featureCloudSource->points[iterator_->queryId].y << ", " << featureCloudSource->points[iterator_->queryId].z);
// ROS_INFO_STREAM("target point " << j++ << ": " << featureCloudTarget->points[iterator_->trainId].x << ", " << featureCloudTarget->points[iterator_->trainId].y << ", " << featureCloudTarget->points[iterator_->trainId].z);
// ROS_INFO("qidx: %d tidx: %d iidx: %d dist: %f", iterator_->queryId, iterator_->trainId, iterator_->imgId, iterator_->distance);
}
/*for (size_t cloudId = 0; cloudId < featureCloudSource->points.size (); ++cloudId)
{
ROS_INFO_STREAM("feature cloud: " << cloudId << ": " << featureCloudSource->points[cloudId].x << "; " << featureCloudSource->points[cloudId].y << "; " << featureCloudSource->points[cloudId].z);
}*/
Eigen::Matrix4f ransacInverse = initialTransform.inverse();
transformPointCloud (*cloud_source, *cloud_ransac_estimation, ransacInverse);
ROS_INFO("Writing point clouds");
filename << "cloud" << i << "DenseSource.pcd";
writer.write (filename.str(), *cloud_source, false);
filename.str("");
filename << "cloud" << i << "DenseTarget.pcd";
writer.write (filename.str(), *cloud_target, true);
filename.str("");
filename << "cloud" << i << "RANSAC-" << (int)indicesSource.size() << "-inliers.pcd";
writer.write (filename.str(), *cloud_ransac_estimation, true);
filename.str("");
filename << "cloud" << i << "SparseSource.pcd";
writer.write (filename.str(), *featureCloudSource, false);
filename.str("");
filename << "cloud" << i << "SparseTarget.pcd";
writer.write (filename.str(), *featureCloudTarget, false);
filename.str("");
i++;
// for(std::vector<int>::iterator iterator_ = indicesSource.begin(); iterator_ != indicesSource.end(); ++iterator_) {
// ROS_INFO("source indice: %d", *iterator_);
// }
}
else
i++;
}
bag.close();
}
//template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
template <typename PointT> void
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
{
std::vector< MeshPtr > meshes_vector;
int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
float cell_size = volume_size_ / voxels_x_;
int mesh_counter = 0;
for(int i = 0; i < max_iterations; ++i)
{
PCL_INFO ("Processing cube number %d\n", i);
//Making cloud local
Eigen::Affine3f cloud_transform;
float originX = (tsdf_offsets[i]).x();
float originY = (tsdf_offsets[i]).y();
float originZ = (tsdf_offsets[i]).z();
cloud_transform.linear ().setIdentity ();
cloud_transform.translation ()[0] = -originX;
cloud_transform.translation ()[1] = -originY;
cloud_transform.translation ()[2] = -originZ;
transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
//Get mesh
MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
if(tmp != nullptr)
{
meshes_vector.push_back (tmp);
mesh_counter++;
}
else
{
PCL_INFO ("This cloud returned no faces, we skip it!\n");
continue;
}
//Making cloud global
cloud_transform.translation ()[0] = originX * cell_size;
cloud_transform.translation ()[1] = originY * cell_size;
cloud_transform.translation ()[2] = originZ * cell_size;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
std::stringstream name;
name << "mesh_" << mesh_counter << ".ply";
PCL_INFO ("Saving mesh...%d \n", mesh_counter);
pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
}
return;
}
void
pcl::gpu::kinfuLS::CyclicalBuffer::performShift (const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift)
{
// compute new origin and offsets
int offset_x, offset_y, offset_z;
computeAndSetNewCubeMetricOrigin (target_point, offset_x, offset_y, offset_z);
// extract current slice from the TSDF volume (coordinates are in indices! (see fetchSliceAsCloud() )
DeviceArray<PointXYZ> points;
DeviceArray<float> intensities;
int size;
if(!last_shift)
{
size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, offset_x, offset_y, offset_z);
}
else
{
size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1);
}
points = DeviceArray<PointXYZ> (cloud_buffer_device_xyz_.ptr (), size);
intensities = DeviceArray<float> (cloud_buffer_device_intensities_.ptr(), size);
PointCloud<PointXYZI>::Ptr current_slice (new PointCloud<PointXYZI>);
PointCloud<PointXYZ>::Ptr current_slice_xyz (new PointCloud<PointXYZ>);
PointCloud<PointIntensity>::Ptr current_slice_intensities (new PointCloud<PointIntensity>);
// Retrieving XYZ
points.download (current_slice_xyz->points);
current_slice_xyz->width = (int) current_slice_xyz->points.size ();
current_slice_xyz->height = 1;
// Retrieving intensities
// TODO change this mechanism by using PointIntensity directly (in spite of float)
// when tried, this lead to wrong intenisty values being extracted by fetchSliceAsCloud () (padding pbls?)
std::vector<float , Eigen::aligned_allocator<float> > intensities_vector;
intensities.download (intensities_vector);
current_slice_intensities->points.resize (current_slice_xyz->points.size ());
for(int i = 0 ; i < current_slice_intensities->points.size () ; ++i)
current_slice_intensities->points[i].intensity = intensities_vector[i];
current_slice_intensities->width = (int) current_slice_intensities->points.size ();
current_slice_intensities->height = 1;
// Concatenating XYZ and Intensities
pcl::concatenateFields (*current_slice_xyz, *current_slice_intensities, *current_slice);
current_slice->width = (int) current_slice->points.size ();
current_slice->height = 1;
// transform the slice from local to global coordinates
Eigen::Affine3f global_cloud_transformation;
global_cloud_transformation.translation ()[0] = buffer_.origin_GRID_global.x;
global_cloud_transformation.translation ()[1] = buffer_.origin_GRID_global.y;
global_cloud_transformation.translation ()[2] = buffer_.origin_GRID_global.z;
global_cloud_transformation.linear () = Eigen::Matrix3f::Identity ();
transformPointCloud (*current_slice, *current_slice, global_cloud_transformation);
// retrieve existing data from the world model
PointCloud<PointXYZI>::Ptr previously_existing_slice (new PointCloud<PointXYZI>);
world_model_.getExistingData (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z,
offset_x, offset_y, offset_z,
buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1,
*previously_existing_slice);
//replace world model data with values extracted from the TSDF buffer slice
world_model_.setSliceAsNans (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z,
offset_x, offset_y, offset_z,
buffer_.voxels_size.x, buffer_.voxels_size.y, buffer_.voxels_size.z);
PCL_INFO ("world contains %d points after update\n", world_model_.getWorldSize ());
world_model_.cleanWorldFromNans ();
PCL_INFO ("world contains %d points after cleaning\n", world_model_.getWorldSize ());
// clear buffer slice and update the world model
pcl::device::kinfuLS::clearTSDFSlice (volume->data (), &buffer_, offset_x, offset_y, offset_z);
// insert current slice in the world if it contains any points
if (current_slice->points.size () != 0) {
world_model_.addSlice(current_slice);
}
// shift buffer addresses
shiftOrigin (volume, offset_x, offset_y, offset_z);
// push existing data in the TSDF buffer
if (previously_existing_slice->points.size () != 0 ) {
volume->pushSlice(previously_existing_slice, getBuffer () );
}
}
int main(int argc, char *argv[]) {
if (argc != 2)
return 0;
Frame3D frames[8];
for (int i = 0; i < 8; ++i) {
frames[i].load(boost::str(boost::format("%s/%05d.3df") % argv[1] % i));
}
std::cout << "Merging point cloud" << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr model_point_cloud_norm = merge(frames);
std::cout << "Got: " << model_point_cloud_norm->size() << " points" << std::endl;
std::cout << "Generating mesh" << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr reduced_point_cloud(new pcl::PointCloud<pcl::PointNormal>());
pcl::PassThrough<pcl::PointNormal> filter;
filter.setInputCloud(model_point_cloud_norm);
filter.filter(*reduced_point_cloud);
std::cout << "Got: " << reduced_point_cloud->size() << " points" << std::endl;
pcl::Poisson<pcl::PointNormal> rec;
rec.setDepth(10);
rec.setInputCloud(reduced_point_cloud);
pcl::PolygonMesh triangles;
rec.reconstruct(triangles);
std::cout << "Finished" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
for (int i = 0; i < 8; ++i) {
float focal_length = frames[i].focal_length_;
// Camera width
double sizeX = frames[i].depth_image_.cols;
// Camera height
double sizeY = frames[i].depth_image_.rows;
// Centers
double cx = sizeX / 2.0;
double cy = sizeY / 2.0;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud = transformPointCloud(cloud, frames[i].getEigenTransform().inverse());
const cv::Mat& zbuffer = computeZbuffer(*transformed_cloud, frames[i]);
int point_found = 0;
for (const pcl::Vertices& polygon : triangles.polygons) {
const pcl::PointXYZRGB& point = transformed_cloud->at(polygon.vertices[0]);
int u_unscaled = std::round(focal_length * (point.x / point.z) + cx);
int v_unscaled = std::round(focal_length * (point.y / point.z) + cy);
const cv::Vec3f& zmap_point = zbuffer.at<cv::Vec3f>(v_unscaled, u_unscaled);
const float eps = 0.000000001;
// If not visible
if (std::fabs(zmap_point[0] - point.x) > eps
|| std::fabs(zmap_point[1] - point.y) > eps
|| std::fabs(zmap_point[2] - point.z) > eps)
continue;
float u = static_cast<float>(u_unscaled / sizeX);
float v = static_cast<float>(v_unscaled / sizeY);
if (u < 0. || u >= 1 || v < 0. || v >= 1)
continue;
int x = std::floor(frames[i].rgb_image_.cols * u);
int y = std::floor(frames[i].rgb_image_.rows * v);
for (int h = 0; h < 3; ++h) {
pcl::PointXYZRGB& original_point = cloud->at(polygon.vertices[h]);
const cv::Vec3b& rgb = frames[i].rgb_image_.at<cv::Vec3b>(y, x);
if (original_point.r != 0 && original_point.g != 0 && original_point.b != 0)
continue;
original_point.b = rgb[0];
original_point.g = rgb[1];
original_point.r = rgb[2];
}
++point_found;
}
std::cout << point_found << " points found" << std::endl;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr textured_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
textured_cloud->reserve(cloud->size());
for (const pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0) {
textured_cloud->push_back(point);
}
}
// Filling gaps
pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree2->setInputCloud(textured_cloud);
for (pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0)
continue;
std::vector<int> k_indices;
std::vector<float> k_dist;
tree2->nearestKSearch(point, 1, k_indices, k_dist);
const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[0]);
point.r = textured_point.r;
point.g = textured_point.g;
point.b = textured_point.b;
}
// Smoothing
tree2->setInputCloud(cloud);
for (pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0)
continue;
std::vector<int> k_indices;
std::vector<float> k_dist;
tree2->nearestKSearch(point, 5, k_indices, k_dist);
int r = 0;
int g = 0;
int b = 0;
for (int i = 0; i < 5; ++i) {
const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[i]);
r += textured_point.r;
g += textured_point.g;
b += textured_point.b;
}
r /= 5;
g /= 5;
b /= 5;
point.r = (uint8_t) r;
point.g = (uint8_t) g;
point.b = (uint8_t) b;
}
pcl::toPCLPointCloud2(*cloud, triangles.cloud);
std::cout << "Finished texturing" << std::endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(1, 1, 1);
viewer->addPolygonMesh(triangles, "meshes", 0);
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
while (!viewer->wasStopped()) {
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
void
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
double newOriginX = previous_origin_x + offset_x;
double newOriginY = previous_origin_y + offset_y;
double newOriginZ = previous_origin_z + offset_z;
double newLimitX = newOriginX + volume_x;
double newLimitY = newOriginY + volume_y;
double newLimitZ = newOriginZ + volume_z;
// filter points in the space of the new cube
PointCloudPtr newCube (new pcl::PointCloud<PointT>);
// condition
ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
// build the filter
pcl::ConditionalRemoval<PointT> condremAND (true);
condremAND.setCondition (range_condAND);
condremAND.setInputCloud (world_);
condremAND.setKeepOrganized (false);
// apply filter
condremAND.filter (*newCube);
// filter points that belong to the new slice
ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
if(offset_x >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
if(offset_y >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
if(offset_z >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
// build the filter
pcl::ConditionalRemoval<PointT> condrem (true);
condrem.setCondition (range_condOR);
condrem.setInputCloud (newCube);
condrem.setKeepOrganized (false);
// apply filter
condrem.filter (existing_slice);
if(existing_slice.points.size () != 0)
{
//transform the slice in new cube coordinates
Eigen::Affine3f transformation;
transformation.translation ()[0] = newOriginX;
transformation.translation ()[1] = newOriginY;
transformation.translation ()[2] = newOriginZ;
transformation.linear ().setIdentity ();
transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
}
}
void Visualization::resetData(const PlanningPipeline& dp, const GraspAnalysis& ref,
const GraspAnalysis& trgt_templt)
{
boost::mutex::scoped_lock lock(mutex_);
/* target_normals */
string frame_id = dp.target_object_.header.frame_id;
target_normals_ = dp.templt_generator_->getVisualizationNormals("target_obj_normals_viz", frame_id);
/* viewpoint */
viewpoint_pose_.header.stamp = ros::Time::now();
viewpoint_pose_.header.frame_id = dp.templt_generator_->frameBase();
viewpoint_pose_.pose.orientation.w = dp.templt_generator_->viewp_rot_.w();
viewpoint_pose_.pose.orientation.x = dp.templt_generator_->viewp_rot_.x();
viewpoint_pose_.pose.orientation.y = dp.templt_generator_->viewp_rot_.y();
viewpoint_pose_.pose.orientation.z = dp.templt_generator_->viewp_rot_.z();
viewpoint_pose_.pose.position.x = dp.templt_generator_->viewp_trans_.x();
viewpoint_pose_.pose.position.y = dp.templt_generator_->viewp_trans_.y();
viewpoint_pose_.pose.position.z = dp.templt_generator_->viewp_trans_.z();
table_pose_.header.stamp = ros::Time::now();
table_pose_.header.frame_id = dp.templt_generator_->frameBase();
table_pose_.pose = dp.templt_generator_->table_pose_;
sensor_msgs::PointCloud2 pc2_tmp;
pcl::toROSMsg(dp.templt_generator_->getConvexHullPoints(), pc2_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(pc2_tmp, target_hull_);
computeHullMesh(dp.templt_generator_->getConvexHullPoints(), dp.templt_generator_->getConvexHullVertices());
pcl::toROSMsg(dp.templt_generator_->getSearchPoints(), pc2_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(pc2_tmp, search_points_);
sensor_msgs::ChannelFloat32 sp_intens;
sp_intens.name = "intensity";
sp_intens.values.resize(search_points_.points.size());
for (unsigned int i = 0; i < search_points_.points.size(); i++)
{
sp_intens.values[i] = i;
}
search_points_.channels.push_back(sp_intens);
/* target object */
sensor_msgs::convertPointCloud2ToPointCloud(dp.target_object_, target_object_);
/* reference object */
dp.getRelatedObject(ref, pc2_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(pc2_tmp, matching_object_);
/* target and matching gripper-poses */
matching_gripper_ = ref.gripper_pose;
target_gripper_ = trgt_templt.gripper_pose;
/* target template */
GraspTemplate t(ref.grasp_template, ref.template_pose.pose);
DismatchMeasure match_handl(t, matching_gripper_.pose);
GraspTemplate current_candidate(trgt_templt.grasp_template, trgt_templt.template_pose.pose);
match_handl.applyDcMask(current_candidate);
target_templt_ = current_candidate.getVisualization("grasp_decision_target_viz", target_object_.header.frame_id);
/* target templt */
target_templt_pose_.header.stamp = ros::Time::now();
target_templt_pose_.header.frame_id = target_object_.header.frame_id;
target_templt_pose_.pose.position.x = current_candidate. object_to_template_translation_.x();
target_templt_pose_.pose.position.y = current_candidate. object_to_template_translation_.y();
target_templt_pose_.pose.position.z = current_candidate. object_to_template_translation_.z();
target_templt_pose_.pose.orientation.x = current_candidate. object_to_template_rotation_.x();
target_templt_pose_.pose.orientation.y = current_candidate. object_to_template_rotation_.y();
target_templt_pose_.pose.orientation.z = current_candidate. object_to_template_rotation_.z();
target_templt_pose_.pose.orientation.w = current_candidate. object_to_template_rotation_.w();
/* target heightmap */
target_hm_
= current_candidate.heightmap_.getVisualization("templt_comp_target", ref.template_pose.header.frame_id, 1);
/* matching template */
matching_templt_ = match_handl.getLibTemplt().getVisualization("grasp_decision_match_viz",
ref.template_pose.header.frame_id);
/* matching heightmap */
matching_hm_ = t.heightmap_.getVisualization("templt_comp_match", ref.template_pose.header.frame_id);
/* transform matchings for better visualization */
Vector3d viz_trans(0, -0.5, 0.5);
Quaterniond viz_rot = Quaterniond::Identity();
transformMarkers(matching_templt_, viz_trans, viz_rot);
transformPose(matching_gripper_.pose, viz_trans, viz_rot);
transformPointCloud(matching_object_, viz_trans, viz_rot);
viz_trans.x() = matching_gripper_.pose.position.x;
viz_trans.y() = matching_gripper_.pose.position.y + 0.35;
viz_trans.z() = matching_gripper_.pose.position.z - 0.2;
adaptForTemplateComparison(matching_hm_, target_hm_, viz_trans, Quaterniond::Identity());
/* visualize gripper in form of a mesh */
string gripper_arch;
double gripper_state = 0;
ros::param::get("~hand_architecture", gripper_arch);
if (gripper_arch == "armrobot" && ref.fingerpositions.vals.size() >= 1)
{
gripper_state = ref.fingerpositions.vals[0];
}
else
{
gripper_state = ref.gripper_joint_state;
}
matching_gripper_mesh_ = visualizeGripper("matching_gripper_mesh",
matching_object_.header.frame_id, matching_gripper_.pose, gripper_state);
target_gripper_mesh_ = visualizeGripper("target_gripper_mesh",
target_object_.header.frame_id, target_gripper_.pose, gripper_state);
}
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudTarget input_corresp;
input_corresp.points.resize (indices_->size ());
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Resize the vector of distances between correspondences
std::vector<float> previous_correspondence_distances (indices_->size ());
correspondence_distances_.resize (indices_->size ());
while (!converged_) // repeat until convergence
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// And the previous set of distances
previous_correspondence_distances = correspondence_distances_;
int cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// Iterating over the entire index vector and find all correspondences
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!this->searchForNeighbors (output, (*indices_)[idx], 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_)[idx]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
source_indices[cnt] = (*indices_)[idx];
target_indices[cnt] = nn_indices[0];
cnt++;
}
// Save the nn_dists[0] to a global vector of distances
correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], static_cast<float> (dist_threshold));
}
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
// Resize to the actual number of valid correspondences
source_indices.resize (cnt); target_indices.resize (cnt);
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
sac.setMaxIterations (ransac_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
source_indices_good = source_indices;
target_indices_good = target_indices;
}
else
{
std::vector<int> inliers;
// Get the inliers
sac.getInliers (inliers);
source_indices_good.resize (inliers.size ());
target_indices_good.resize (inliers.size ());
boost::unordered_map<int, int> source_to_target;
for (unsigned int i = 0; i < source_indices.size(); ++i)
source_to_target[source_indices[i]] = target_indices[i];
// Copy just the inliers
std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
for (size_t i = 0; i < inliers.size (); ++i)
target_indices_good[i] = source_to_target[inliers[i]];
}
}
// Check whether we have enough correspondences
cnt = static_cast<int> (source_indices_good.size ());
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %zu points [100.0%%], RANSAC rejected: %zu [%f%%].\n",
getClassName ().c_str (),
cnt,
(static_cast<float> (cnt) * 100.0f) / static_cast<float> (indices_->size ()),
indices_->size (),
source_indices.size () - cnt,
static_cast<float> (source_indices.size () - cnt) * 100.0f / static_cast<float> (source_indices.size ()));
// Estimate the transform
//rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);
// Tranform the data
transformPointCloud (output, output, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
nr_iterations_++;
// Update the vizualization of icp convergence
if (update_visualizer_ != 0)
update_visualizer_(output, source_indices_good, *target_, target_indices_good );
// Various/Different convergence termination criteria
// 1. Number of iterations has reached the maximum user imposed number of iterations (via
// setMaximumIterations)
// 2. The epsilon (difference) between the previous transformation and the current estimated transformation
// is smaller than an user imposed value (via setTransformationEpsilon)
// 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via
// setEuclideanFitnessEpsilon)
if (nr_iterations_ >= max_iterations_ ||
(transformation_ - previous_transformation_).array ().abs ().sum () < transformation_epsilon_ ||
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
)
{
converged_ = true;
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 ());
PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
PCL_DEBUG ("(transformation_ - previous_transformation_).array ().abs ().sum () (%f) < transformation_epsilon_ (%f)\n",
(transformation_ - previous_transformation_).array ().abs ().sum (), transformation_epsilon_);
PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
euclidean_fitness_epsilon_);
}
}
}
void transformPointCloud(PointCloudConstPtr input_cloud_ptr, PointCloudPtr output_cloud_ptr, const Eigen::Matrix4f& transform )
{
transformPointCloud (*input_cloud_ptr, *output_cloud_ptr, transform);
}
int TYCloudMatcher::bestDirection(const std::vector<cv::Point3f> &pCloud, const cv::Point3f ¢er){
Vec6f argPeek;
float min = FLT_MAX;
float dist;
int idx = -1;
if(!isFirstIter && isPreIdxValid ){
for(int j = 0 ; j < 3 ; j++) argPeek[j] = argCurrent[j] + gradient[preIterIdx][j] * rStep;
for(int j = 3 ; j < 6 ; j++) argPeek[j] = argCurrent[j] + gradient[preIterIdx][j] * tStep;
/* Translate and rotate the source point cloud by the current argument */
transformPointCloud(argPeek, pCloud, center, this->bufCloud, this->bufCenter);
/* Calculate Energy(Distance) */
dist = energy(this->bufCloud);
/* Previous Gradient Doesn't Work Anymore */
if( dist >= preIterEnergy ){
isPreIdxValid = false;
}else{
preIterEnergy = dist;
idx = preIterIdx;
min = dist;
}
}
if(isFirstIter || !isPreIdxValid){
/* Try 13 Directions, For Each Direction: */
for(int i = 0 ; i < 13 ; i ++){
/* Set up the peaking argument for the current direction */
if(i > 6) // i = [7,12] : is Translating direction
for(int j = 0 ; j < 6 ; j++) argPeek[j] = argCurrent[j] + gradient[i][j] * tStep;
else if(i > 0) // i = [1, 6] : is Rotating direction
for(int j = 0 ; j < 6 ; j++) argPeek[j] = argCurrent[j] + gradient[i][j] * rStep;
else // i = 0 : is No Transforming direction
for(int j = 0 ; j < 6 ; j++) argPeek[j] = argCurrent[j];
/* Translate and rotate the source point cloud by the current argument */
transformPointCloud(argPeek, pCloud, center, this->bufCloud, this->bufCenter);
/* Calculate Energy(Distance) */
dist = energy(this->bufCloud);
/* Keep the best one */
if( dist < min ){
min = dist;
idx = i;
}
}
if(idx == 0) isPreIdxValid = false;
else{
isPreIdxValid = true;
preIterEnergy = min;
preIterIdx = idx;
}
}
float step = (idx>6) ? tStep:rStep;
for(int i = 0 ; i < 6 ; i ++)
argCurrent[i] = argCurrent[i] + gradient[idx][i]*step;
if(isFirstIter){
perPointEnergy = min / (float)pCloud.size();
}else{
float curEPP = min / (float)pCloud.size();
graOfPerPointEnergy = perPointEnergy - curEPP;
perPointEnergy = curEPP;
if(graOfPerPointEnergy < 0) {
printf("Exception : TYCloudMatcher::bestDirection \"graOfPerPointEnergy < 0\" \n");
}
}
if(perPointEnergy > 50) printf("EPP: %.2f, Energy: %.2f\n", perPointEnergy, min);
/*if(min > 1000) printf("EPP: %.2f, Energy: %.2f\n", perPointEnergy, min);
else printf("EPP: %.2f\n", perPointEnergy);
else
if(min > 1000) printf("Energy: %.2f\n", min);
*/
//printf("Step:(%.1f,%.1f,%.1f,%.1f,%.1f,%.1f), (idx, min) = (%d, %.2f, %.2f) \n",
//argCurrent[0], argCurrent[1], argCurrent[2], argCurrent[3], argCurrent[4], argCurrent[5], idx, min, perPointEnergy);
//preIterEnergy = min;
//preIterIdx = idx;
return idx;
};
