/* 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; }