/* decide which extrusion example to run */
static void
chooseExtrusionExample (ModeInfo *mi)
{
extrusionstruct *gp = &Extrusion[MI_SCREEN(mi)];
int i;
/* call the extrusion init routine */
if (!strncmp(which_name, "RANDOM", strlen(which_name))) {
gp->extrusion_number = random() % num_extrusions;
}
else {
gp->extrusion_number=-1;
for (i=0; i < num_extrusions; i++) {
if (!strncmp(which_name, funcs_ptr[i].name, strlen(which_name))) {
gp->extrusion_number = i;
}
}
}
if (gp->extrusion_number < 0 || gp->extrusion_number >= num_extrusions) {
fprintf(stderr, "%s: invalid extrusion example number!\n", progname);
fprintf(stderr, "%s: known extrusions:\n", progname);
for (i=0; i < num_extrusions; i++)
fprintf(stderr,"\t%s\n", funcs_ptr[i].name);
exit(1);
}
init_rotation(mi);
funcs_ptr[gp->extrusion_number].InitStuff();
}
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 ());
}
int
main (int argc, char** argv)
{
// Loading first scan of room.
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
// Loading second scan of room from new perspective.
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
// Filtering input scan to roughly 10% of original size to increase speed of registration.
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
// Initializing Normal Distributions Transform (NDT).
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon (0.01);
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize (0.1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution (1.0);
// Setting max number of registration iterations.
ndt.setMaximumIterations (35);
// Setting point cloud to be aligned.
ndt.setInputCloud (filtered_cloud);
// Setting point cloud to be aligned to.
ndt.setInputTarget (target_cloud);
// Set initial alignment estimate found using robot odometry.
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
// Calculating required rigid transform to align the input cloud to the target cloud.
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
// Transforming unfiltered, input cloud using found transform.
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
// Saving transformed input cloud.
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// Initializing point cloud visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0);
// Coloring and visualizing target cloud (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
// Coloring and visualizing transformed input cloud (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// Starting visualizer
viewer_final->addCoordinateSystem (1.0);
viewer_final->initCameraParameters ();
// Wait until visualizer window is closed.
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
/* Fill a module object from a json module dictionary that has
* keys matching the module proerty names
*/
bool
ckbot::fill_module(const Json::Value& json_mod, ckbot::module_link* module)
{
ckbot::module_description this_module_desc;
double damping = json_mod["damping"].asDouble();
double rotor_cog_mag = json_mod.get("rotor_cogging", 0.0).asDouble();
double stator_cog_mag = json_mod.get("stator_cogging", 0.0).asDouble();
double cogging_offset = json_mod.get("cogging_offset", 0.0).asDouble();
double mass = json_mod["mass"].asDouble();
double joint_max = json_mod["joint_max"].asDouble();
double joint_min = json_mod["joint_min"].asDouble();
double torque_max = json_mod["torque_max"].asDouble();
Eigen::Vector3d forward_joint_axis;
Eigen::Vector3d r_ip1;
Eigen::Vector3d r_im1;
if ((json_mod["f_jt_axis"].size() != 3)
|| (json_mod["r_im1"].size() != 3)
|| (json_mod["r_ip1"].size() != 3))
{
return false;
}
for (unsigned int m=0; m<3; ++m)
{
/*Need 0u as index to distinguish from operator[] which takes a string*/
forward_joint_axis(m) = ((json_mod["f_jt_axis"])[m])[0u].asDouble();
r_im1(m) = ((json_mod["r_im1"])[m])[0u].asDouble();
r_ip1(m) = ((json_mod["r_ip1"])[m])[0u].asDouble();
}
Eigen::Matrix3d I_cm;
Eigen::Matrix3d R_jts;
Eigen::Matrix3d init_rotation;
/* TODO: Check that the json for these 3 are 3x3 arrays!!! */
for (unsigned int m=0; m < 3; ++m)
{
for (unsigned int n=0; n<3; ++n)
{
I_cm(m,n) = json_mod["I_cm"][m][n].asDouble();
R_jts(m,n) = json_mod["R_jts"][m][n].asDouble();
init_rotation(m,n) = json_mod["init_rotation"][m][n].asDouble();
}
}
this_module_desc.damping = damping;
this_module_desc.rotor_cogging = rotor_cog_mag;
this_module_desc.stator_cogging = stator_cog_mag;
this_module_desc.cogging_offset = cogging_offset;
this_module_desc.m = mass;
this_module_desc.joint_max = joint_max;
this_module_desc.joint_min = joint_min;
this_module_desc.torque_max = torque_max;
this_module_desc.forward_joint_axis = forward_joint_axis;
this_module_desc.r_im1 = r_im1;
this_module_desc.r_ip1 = r_ip1;
this_module_desc.I_cm = I_cm;
this_module_desc.R_jts = R_jts;
this_module_desc.init_rotation = init_rotation;
ckbot::module_link* this_module = new ckbot::module_link(this_module_desc);
*module = *this_module;
return true;
}
