35 #define BOOST_PARAMETER_MAX_ARITY 7 40 #include <pcl/common/transforms.h> 41 #include <pcl/filters/extract_indices.h> 42 #include <jsk_recognition_msgs/ICPAlign.h> 52 const Eigen::Affine3f& pose):
59 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
70 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
82 pcl::PointCloud<pcl::PointXYZRGB> cloud)
95 DiagnosticNodelet::onInit();
97 std::string(
"multisense/left_camera_optical_frame"));
98 start_registration_srv_
99 = pnh_->advertiseService(
102 pub_cloud_non_registered_
103 = pnh_->advertise<sensor_msgs::PointCloud2>(
"output/non_registered", 1);
105 = pnh_->advertise<sensor_msgs::PointCloud2>(
"output/registered", 1);
106 sub_cloud_.subscribe(*pnh_,
"input", 1);
107 sub_indices_.subscribe(*pnh_,
"input/indices", 1);
108 sub_pose_.subscribe(*pnh_,
"input/pose", 1);
109 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
110 sync_->connectInput(sub_cloud_, sub_indices_, sub_pose_);
111 sync_->registerCallback(boost::bind(
118 pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
119 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output,
120 const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
122 Eigen::Affine3f posef;
124 Eigen::Affine3f transform = posef.inverse();
126 pcl::transformPointCloud<pcl::PointXYZRGB>(
127 *input, *output, transform);
131 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
132 const pcl_msgs::PointIndices::ConstPtr& indices_msg,
133 const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
136 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
137 cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
138 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
139 filtered_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
141 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
142 transformed_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
143 pcl::PointIndices::Ptr
144 indices (
new pcl::PointIndices);
145 indices->indices = indices_msg->indices;
146 pcl::ExtractIndices<pcl::PointXYZRGB>
ex;
147 ex.setInputCloud(cloud);
148 ex.setIndices(indices);
149 ex.filter(*filtered_cloud);
150 transformPointCloudRepsectedToPose(
151 filtered_cloud, transformed_cloud, pose_msg);
152 Eigen::Affine3f initial_pose;
153 if (samples_.size() == 0) {
156 initial_pose = origin_;
160 Eigen::Affine3f offset;
162 initial_pose = origin_.inverse() * offset;
165 samples_.push_back(sample);
167 all_cloud_ = all_cloud_ + *transformed_cloud;
168 sensor_msgs::PointCloud2 ros_all_cloud;
170 ros_all_cloud.header = cloud_msg->header;
171 pub_cloud_non_registered_.publish(ros_all_cloud);
175 pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference,
176 pcl::PointCloud<pcl::PointXYZRGB>::Ptr target,
177 Eigen::Affine3f& output_transform)
180 = pnh_->serviceClient<jsk_recognition_msgs::ICPAlign>(
"icp_service");
181 sensor_msgs::PointCloud2 reference_ros, target_ros;
185 reference_ros.header.stamp = target_ros.header.stamp =
now;
186 reference_ros.header.frame_id = target_ros.header.frame_id =
"map";
187 jsk_recognition_msgs::ICPAlign
srv;
188 srv.request.reference_cloud = reference_ros;
189 srv.request.target_cloud = target_ros;
196 std_srvs::Empty::Request&
req,
197 std_srvs::Empty::Response&
res)
199 if (samples_.size() <= 1) {
203 ROS_INFO(
"Starting registration %lu samples", samples_.size());
206 initial_sample->setRefinedPointCloud(
207 *(initial_sample->getOriginalPointCloud()));
208 initial_sample->setRefinedPose(initial_sample->getOriginalPose());
209 for (
size_t i = 0; i < samples_.size() - 1; i++) {
212 pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference_cloud
213 = from_sample->getRefinedPointCloud();
214 pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_cloud
215 = to_sample->getOriginalPointCloud();
216 Eigen::Affine3f transform;
217 callICP(reference_cloud, target_cloud, transform);
218 to_sample->setRefinedPose(to_sample->getOriginalPose() * transform);
220 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud
221 (
new pcl::PointCloud<pcl::PointXYZRGB>);
222 pcl::transformPointCloud<pcl::PointXYZRGB>(
223 *target_cloud, *transformed_cloud, transform);
224 to_sample->setRefinedPointCloud(*transformed_cloud);
226 pcl::PointCloud<pcl::PointXYZRGB>::Ptr registered_cloud
227 (
new pcl::PointCloud<pcl::PointXYZRGB>);
228 pcl::PointCloud<pcl::PointXYZRGB>::Ptr non_registered_cloud
229 (
new pcl::PointCloud<pcl::PointXYZRGB>);
230 for (
size_t i = 0; i < samples_.size(); i++) {
231 *registered_cloud = *(samples_[i]->getRefinedPointCloud()) + *registered_cloud;
232 *non_registered_cloud = *(samples_[i]->getOriginalPointCloud()) + *non_registered_cloud;
234 sensor_msgs::PointCloud2 registered_ros_cloud, nonregistered_ros_cloud;
237 registered_ros_cloud.header.frame_id =
frame_id_;
238 pub_registered_.publish(registered_ros_cloud);
239 pcl::toROSMsg(*non_registered_cloud, nonregistered_ros_cloud);
241 nonregistered_ros_cloud.header.frame_id =
frame_id_;
242 pub_cloud_non_registered_.publish(nonregistered_ros_cloud);
pcl::PointCloud< pcl::PointXYZRGB >::Ptr original_cloud_
virtual void newsampleCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const pcl_msgs::PointIndices::ConstPtr &indices_msg, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::IncrementalModelRegistration, nodelet::Nodelet)
virtual void setRefinedPointCloud(pcl::PointCloud< pcl::PointXYZRGB > cloud)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
virtual Eigen::Affine3f getRefinedPose()
bool call(MReq &req, MRes &res)
Eigen::Affine3f original_pose_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual bool startRegistration(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
virtual void transformPointCloudRepsectedToPose(pcl::PointCloud< pcl::PointXYZRGB >::Ptr input, pcl::PointCloud< pcl::PointXYZRGB >::Ptr output, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getOriginalPointCloud()
CapturedSamplePointCloud()
sensor_msgs::PointCloud2 PointCloud
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getRefinedPointCloud()
pcl::PointCloud< pcl::PointXYZRGB >::Ptr refined_cloud_
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual Eigen::Affine3f getOriginalPose()
virtual void setRefinedPose(Eigen::Affine3f pose)
virtual void callICP(pcl::PointCloud< pcl::PointXYZRGB >::Ptr reference, pcl::PointCloud< pcl::PointXYZRGB >::Ptr target, Eigen::Affine3f &output_transform)
Eigen::Affine3f refined_pose_