void ObjectTracker :: addNewDetection(const ObjectMatch& match) { m_raw_pose = match.pose()->pose3d(); cv::Vec3f r = match.pose()->pose3d().cvRodriguesRotation(); cv::Vec3f t = match.pose()->pose3d().cvTranslation(); cv::Vec3f dr = r - m_estimated_pose.cvRodriguesRotation(); cv::Vec3f dt = t - m_estimated_pose.cvTranslation(); cv::Mat1f measurement(12,1); measurement << t[0], dt[0], t[1], dt[1], t[2], dt[2], r[0], dr[0], r[1], dr[1], r[2], dr[2]; Mat1f new_state = m_kalman.correct(measurement); m_estimated_pose.resetCameraTransform(); m_estimated_pose.applyTransformBeforeRodrigues(Vec3f(new_state(0,0), new_state(2,0), new_state(4,0)), Vec3f(new_state(6,0), new_state(8,0), new_state(10,0))); ntk_dbg_print(m_last_pose, 1); ntk_dbg_print(m_estimated_pose , 1); ntk_dbg_print(m_raw_pose , 1); m_last_projected_bounding_rect = bounding_box(match.projectedBoundingRect()); m_nframes_without_detection = 0; ++m_nframes_with_detection; m_has_updated_pose = true; }
bool RelativePoseEstimatorICP<PointT> :: preprocessClouds() { if (m_target_cloud->points.size() < 1) { ntk_dbg(1) << "Target cloud was empty"; return false; } PointCloudConstPtr target = m_target_cloud; PointCloudConstPtr source = m_source_cloud; ntk_dbg_print(target->points.size(), 1); ntk_dbg_print(source->points.size(), 1); m_filtered_source.reset(new pcl::PointCloud<PointT>()); m_filtered_target.reset(new pcl::PointCloud<PointT>()); PointCloudPtr temp_source (new pcl::PointCloud<PointT>()); PointCloudPtr temp_target (new pcl::PointCloud<PointT>()); pcl::PassThrough<PointT> filter; filter.setInputCloud (source); filter.filter (*temp_source); filter.setInputCloud (target); filter.filter (*temp_target); pcl::VoxelGrid<PointT> grid; grid.setLeafSize (m_voxel_leaf_size, m_voxel_leaf_size, m_voxel_leaf_size); grid.setInputCloud(temp_source); grid.filter(*m_filtered_source); grid.setInputCloud(temp_target); grid.filter(*m_filtered_target); ntk_dbg_print(temp_source->points.size(), 1); ntk_dbg_print(m_filtered_source->points.size(), 1); ntk_dbg_print(m_filtered_target->points.size(), 1); if (m_filtered_source->points.size() < 1 || m_filtered_target->points.size() < 1) { ntk_dbg(1) << "Warning: no remaining points after filtering."; return false; } if (m_initial_pose.isValid()) { Eigen::Affine3f H = toPclInvCameraTransform(m_initial_pose); this->transformPointCloud(*m_filtered_source, *m_filtered_source, H); } if (m_target_pose.isValid()) { Eigen::Affine3f H = toPclInvCameraTransform(m_target_pose); this->transformPointCloud(*m_filtered_target, *m_filtered_target, H); } return true; }
double cv_emd_distance(const std::vector<double>& h1, const std::vector<double>& h2, int histogram_cycle) { int n = h1.size(); #if 0 ntk_dbg_print(h1.size(), 1); ntk_dbg_print(h2.size(), 1); double norm1 = std::accumulate(stl_bounds(h1), 0.0); double norm2 = std::accumulate(stl_bounds(h2), 0.0); ntk_dbg_print(norm1, 1); ntk_dbg_print(norm2, 1); #endif CvMat* sig1 = cvCreateMat(n, 3, CV_32FC1); CvMat* sig2 = cvCreateMat(n, 3, CV_32FC1); foreach_idx(i, h1) { int row = i/histogram_cycle; int col = i%histogram_cycle; cvSet2D(sig1, i, 0, cvScalar(h1[i])); cvSet2D(sig1, i, 1, cvScalar(row)); cvSet2D(sig1, i, 2, cvScalar(col)); cvSet2D(sig2, i, 0, cvScalar(h2[i])); cvSet2D(sig2, i, 1, cvScalar(row)); cvSet2D(sig2, i, 2, cvScalar(col)); }
FeatureIndexerKdt::MatchResults FeatureIndexerKdt :: findMatches(const LocatedFeature& p) const { float min_dist = std::numeric_limits<float>::max(); float min_dist2 = std::numeric_limits<float>::max(); const LocatedFeature* closest_id = 0; const LocatedFeature* closest_id2 = 0; std::vector<float> query; prepareFlannQuery(query, p); std::vector<int> indices(2, -1); std::vector<float> dists(2, 0); { QWriteLocker locker(&m_lock); m_flann_index->knnSearch(query, indices, dists, 2, cv::flann::SearchParams(64)); } MatchResults c; if (indices[0] >= 0 && indices[1] >= 0) { const LocatedFeature* p1 = m_points[indices[0]]; const LocatedFeature* p2 = m_points[indices[1]]; // No muy eficaz if (0 && p.location().has_depth && p1->location().has_depth) { double scale_ratio = (p.location().scale / p1->location().scale); double z_ratio = (p.location().p_image.z / p1->location().p_image.z); double diff = (scale_ratio * z_ratio); if (diff < 1) diff = 1.0 / diff; if (diff > 1.7) // scale does not match { // ++nb_filtered; if ((dists[0]/dists[1]) < 0.5) { ntk_dbg_print(diff, 1); ntk_dbg_print(scale_ratio, 1); ntk_dbg_print(z_ratio, 1); ntk_dbg_print(p.location().p_image.z, 1); ntk_dbg_print(p1->location().p_image.z, 1); } return c; } } dists[1] += 1; // avoid degenerate case where dist is 0. c.matches.push_back(Match(p1, dists[0]/dists[1])); } return c; }
void MeshRenderer :: setPose(const Pose3D& pose, float* arg_near_plane, float* arg_far_plane) { VertexBufferObject& vbo = m_vertex_buffer_object; pose.cvCameraTransform().copyTo(vbo.model_view_matrix); // Transpose the matrix for OpenGL column-major. vbo.model_view_matrix = vbo.model_view_matrix.t(); if (!(arg_near_plane && arg_far_plane)) { estimateOptimalPlanes(pose, &m_last_near_plane, &m_last_far_plane); } else { m_last_near_plane = *arg_near_plane; m_last_far_plane = *arg_far_plane; } m_pbuffer->makeCurrent(); glMatrixMode (GL_MODELVIEW); glLoadIdentity (); cv::Vec3f euler_angles = pose.cvEulerRotation(); glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]); glRotatef(euler_angles[2]*180.0/M_PI, 0, 0, 1); glRotatef(euler_angles[1]*180.0/M_PI, 0, 1, 0); glRotatef(euler_angles[0]*180.0/M_PI, 1, 0, 0); glMatrixMode (GL_PROJECTION); glLoadIdentity (); double dx = pose.imageCenterX() - (m_pbuffer->width() / 2.0); double dy = pose.imageCenterY() - (m_pbuffer->height() / 2.0); glViewport(dx, -dy, m_pbuffer->width(), m_pbuffer->height()); if (pose.isOrthographic()) { ntk_dbg_print(pose.focalX()/2, 0); ntk_dbg_print(pose.focalY()/2, 0); glOrtho(-pose.focalX()/2, pose.focalX()/2, -pose.focalY()/2, pose.focalY()/2, m_last_near_plane, m_last_far_plane); } else { double fov = (180.0/M_PI) * 2.0*atan(m_pbuffer->height()/(2.0*pose.focalY())); // double fov2 = (180.0/M_PI) * 2.0*atan(image.cols/(2.0*pose.focalX())); // ntk_dbg_print(fov2, 2); // gluPerspective(fov2, double(image.rows)/image.cols, near_plane, far_plane); gluPerspective(fov, double(m_pbuffer->width())/m_pbuffer->height(), m_last_near_plane, m_last_far_plane); } glMatrixMode (GL_MODELVIEW); }
bool RGBDGrabberFactory :: createOpenni2Grabbers(const ntk::RGBDGrabberFactory::Params ¶ms, std::vector<GrabberData>& grabbers) { #ifdef NESTK_USE_OPENNI2 if (!Openni2Driver::hasDll()) { ntk_warn("OpenNI2: No dll found.\n"); return false; } // Drivers dll are supposed to be next to the binaries. QDir prev = QDir::current(); QDir::setCurrent(QApplication::applicationDirPath()); ni2_driver = new Openni2Driver(); if (!ni2_driver->isReady()) { ntk_error("OpenNI2: %s\n", ni2_driver->getLastError().toUtf8().constData()); return false; } Openni2Driver::SensorInfos sensorInfos; ni2_driver->getSensorInfos(sensorInfos); ntk_info("OpenNI2: Number of devices: %d\n", sensorInfos.size()); ntk_dbg_print(sensorInfos.size(), 1); // Create grabbers. for (unsigned n = 0; n < sensorInfos.size(); ++n) { Openni2Grabber* grabber = new Openni2Grabber(*ni2_driver, sensorInfos[n].uri); if (params.high_resolution) grabber->setHighRgbResolution(true); grabber->setCustomBayerDecoding(false); grabber->setUseHardwareRegistration(params.hardware_registration); GrabberData data; data.grabber = grabber; data.type = OPENNI2; data.processor = createProcessor(OPENNI2); grabbers.push_back(data); } QDir::setCurrent(prev.absolutePath()); return sensorInfos.size() > 0; #else return false; #endif }
void FreenectGrabber :: run() { setThreadShouldExit(false); m_current_image.setCalibration(m_calib_data); m_rgbd_image.setCalibration(m_calib_data); m_rgbd_image.rawRgbRef() = Mat3b(FREENECT_FRAME_H, FREENECT_FRAME_W); m_rgbd_image.rawDepthRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W); m_rgbd_image.rawIntensityRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W); m_current_image.rawRgbRef() = Mat3b(FREENECT_FRAME_H, FREENECT_FRAME_W); m_current_image.rawDepthRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W); m_current_image.rawIntensityRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W); startKinect(); int64 last_grab_time = 0; while (!threadShouldExit()) { waitForNewEvent(-1); // Use infinite timeout in order to honor sync mode. while (m_depth_transmitted || m_rgb_transmitted) freenect_process_events(f_ctx); // m_current_image.rawDepth().copyTo(m_current_image.rawAmplitudeRef()); // m_current_image.rawDepth().copyTo(m_current_image.rawIntensityRef()); { int64 grab_time = ntk::Time::getMillisecondCounter(); ntk_dbg_print(grab_time - last_grab_time, 2); last_grab_time = grab_time; QWriteLocker locker(&m_lock); // FIXME: ugly hack to handle the possible time // gaps between rgb and IR frames in dual mode. if (m_dual_ir_rgb) m_current_image.copyTo(m_rgbd_image); else m_current_image.swap(m_rgbd_image); m_rgb_transmitted = true; m_depth_transmitted = true; } if (m_dual_ir_rgb) setIRMode(!m_ir_mode); advertiseNewFrame(); #ifdef _WIN32 // FIXME: this is to avoid GUI freezes with libfreenect on Windows. // See http://groups.google.com/group/openkinect/t/b1d828d108e9e69 Sleep(1); #endif } }
void MeshRenderer :: computeProjectionMatrix(cv::Mat4b& image, const Pose3D& pose) { double near_plane, far_plane; estimateOptimalPlanes(pose, &near_plane, &far_plane); ntk_dbg_print(near_plane, 2); ntk_dbg_print(far_plane, 2); m_last_near_plane = near_plane; m_last_far_plane = far_plane; m_pbuffer->makeCurrent(); glMatrixMode (GL_MODELVIEW); glLoadIdentity (); cv::Vec3f euler_angles = pose.cvEulerRotation(); glTranslatef(pose.cvTranslation()[0], pose.cvTranslation()[1], pose.cvTranslation()[2]); glRotatef(euler_angles[2]*180.0/M_PI, 0, 0, 1); glRotatef(euler_angles[1]*180.0/M_PI, 0, 1, 0); glRotatef(euler_angles[0]*180.0/M_PI, 1, 0, 0); glMatrixMode (GL_PROJECTION); glLoadIdentity (); double dx = pose.imageCenterX() - (image.cols / 2.0); double dy = pose.imageCenterY() - (image.rows / 2.0); glViewport(dx, -dy, image.cols, image.rows); //glViewport(0, 0, image.cols, image.rows); if (pose.isOrthographic()) { gluOrtho2D(-pose.focalX()/2, pose.focalX()/2, -pose.focalY()/2, pose.focalY()/2); } else { double fov = (180.0/M_PI) * 2.0*atan(image.rows/(2.0*pose.focalY())); // double fov2 = (180.0/M_PI) * 2.0*atan(image.cols/(2.0*pose.focalX())); // ntk_dbg_print(fov2, 2); // gluPerspective(fov2, double(image.rows)/image.cols, near_plane, far_plane); gluPerspective(fov, double(image.cols)/image.rows, near_plane, far_plane); } glMatrixMode (GL_MODELVIEW); }
void MeshRenderer :: estimateOptimalPlanes(const Pose3D& pose, float* near_plane, float* far_plane) { float min_z = std::numeric_limits<float>::max(); float max_z = 0.01f; for (int i = 0; i < m_mesh->faces.size(); ++i) { const Point3f& v1 = m_mesh->vertices[m_mesh->faces[i].indices[0]]; const Point3f& v2 = m_mesh->vertices[m_mesh->faces[i].indices[1]]; const Point3f& v3 = m_mesh->vertices[m_mesh->faces[i].indices[2]]; Point3f pv1 = pose.cameraTransform(v1); Point3f pv2 = pose.cameraTransform(v2); Point3f pv3 = pose.cameraTransform(v3); min_z = ntk::math::min(min_z,-pv1.z); min_z = ntk::math::min(min_z,-pv2.z); min_z = ntk::math::min(min_z,-pv3.z); max_z = ntk::math::max(max_z,-pv1.z); max_z = ntk::math::max(max_z,-pv2.z); max_z = ntk::math::max(max_z,-pv3.z); } ntk_dbg_print(min_z, 2); ntk_dbg_print(max_z, 2); if (min_z < 0) min_z = 0.01f; if (max_z < min_z) max_z = (min_z*2); *near_plane = min_z*0.9f; *far_plane = max_z*1.1f; }
void triangulate(const Vertices& vertices, PolygonMesh& output) { const int n_vertices = vertices.vertices.size(); if (n_vertices <= 3) { output.polygons.push_back(vertices); return; } std::vector<uint32_t> remaining_vertices(n_vertices); if (area(vertices.vertices) > 0) // clockwise? { remaining_vertices = vertices.vertices; } else { for (int v = 0; v < n_vertices; v++) remaining_vertices[v] = vertices.vertices[n_vertices - 1 - v]; } // Avoid closed loops. if (remaining_vertices.front() == remaining_vertices.back()) remaining_vertices.erase(remaining_vertices.end()-1); // null_iterations avoids infinite loops if the polygon is not simple. for (int u = remaining_vertices.size() - 1, null_iterations = 0; remaining_vertices.size() > 2 && null_iterations < remaining_vertices.size()*2; ++null_iterations, u=(u+1)%remaining_vertices.size()) { int v = (u + 1) % remaining_vertices.size(); int w = (u + 2) % remaining_vertices.size(); if (isEar(u, v, w, remaining_vertices)) { Vertices triangle; triangle.vertices.resize(3); triangle.vertices[0] = remaining_vertices[u]; triangle.vertices[1] = remaining_vertices[v]; triangle.vertices[2] = remaining_vertices[w]; output.polygons.push_back(triangle); remaining_vertices.erase(remaining_vertices.begin()+v); null_iterations = 0; } } for (int i = 0; i < remaining_vertices.size(); ++i) ntk_dbg_print(remaining_vertices[i], 1); }
bool RelativePoseEstimatorICP<PointT> :: estimateNewPose() { bool ok = preprocessClouds(); if (!ok) return false; if (0) { ntk::Mesh mesh1, mesh2; pointCloudToMesh(mesh1, *m_filtered_source); pointCloudToMesh(mesh2, *m_filtered_target); mesh1.saveToPlyFile("debug_mesh_source.ply"); mesh2.saveToPlyFile("debug_mesh_target.ply"); } // The source cloud should be smaller than the target one. bool swap_source_and_target = false; if (m_filtered_source->points.size() > 2.0 * m_filtered_target->points.size()) { swap_source_and_target = true; std::swap(m_filtered_source, m_filtered_target); } Pose3D relative_pose; PointCloudType cloud_reg; ok = computeRegistration(relative_pose, m_filtered_source, m_filtered_target, cloud_reg); if (!ok) return false; if (swap_source_and_target) relative_pose.invert(); ntk_dbg_print(relative_pose.cvTranslation(), 1); #if 0 m_estimated_pose = m_target_pose; m_estimated_pose.applyTransformBefore(relative_pose); m_estimated_pose.applyTransformAfter(m_initial_pose); #endif // Be careful. Poses are actually inverse camera transform in 3D space. // inverse(newpose) = inverse(relative_pose) * inverse(target_pose) m_estimated_pose = m_initial_pose; m_estimated_pose.applyTransformBefore(relative_pose.inverted()); return true; }
void SiftObjectDetector :: initializeFindObjects() { super::initializeFindObjects(); m_point_matches.clear(); m_image_sift_points.clear(); std::list<LocatedFeature*> points; ntk::TimeCount tc_init("initialize", 1); compute_feature_points(points, m_data.image, siftDatabase().featureType()); tc_init.elapsedMsecs(" compute feature points: "); std::copy(stl_bounds(points), std::back_inserter(m_image_sift_points)); ntk_dbg_print(m_image_sift_points.size(), 1); tc_init.stop(); }
bool RGBDGrabberFactory :: createOpenniGrabbers(const ntk::RGBDGrabberFactory::Params ¶ms, std::vector<GrabberData>& grabbers) { #ifdef NESTK_USE_OPENNI if (!OpenniDriver::hasDll()) { ntk_warn("No OpenNI dll found.\n"); return false; } // Config dir is supposed to be next to the binaries. QDir prev = QDir::current(); QDir::setCurrent(QApplication::applicationDirPath()); ni_driver = new OpenniDriver(); ntk_info("Number of Openni devices: %d\n", ni_driver->numDevices()); ntk_dbg_print(ni_driver->numDevices(), 1); // Create grabbers. for (int i = 0; i < ni_driver->numDevices(); ++i) { OpenniGrabber* k_grabber = new OpenniGrabber(*ni_driver, i); k_grabber->setTrackUsers(false); if (params.high_resolution) k_grabber->setHighRgbResolution(true); k_grabber->setCustomBayerDecoding(false); k_grabber->setUseHardwareRegistration(params.hardware_registration); GrabberData new_data; new_data.grabber = k_grabber; new_data.type = OPENNI; new_data.processor = createProcessor(OPENNI); grabbers.push_back(new_data); } QDir::setCurrent(prev.absolutePath()); return ni_driver->numDevices() > 0; #else return false; #endif }
bool RelativePoseEstimatorICP<PointT> :: computeRegistration(Pose3D& relative_pose, PointCloudConstPtr source_cloud, PointCloudConstPtr target_cloud, PointCloudType& aligned_cloud) { pcl::IterativeClosestPoint<PointT, PointT> reg; reg.setMaximumIterations (m_max_iterations); reg.setTransformationEpsilon (1e-10); reg.setRANSACOutlierRejectionThreshold(m_ransac_outlier_threshold); reg.setMaxCorrespondenceDistance (m_distance_threshold); reg.setInputCloud (source_cloud); reg.setInputTarget (target_cloud); reg.align (aligned_cloud); if (0) { ntk::Mesh mesh1, mesh2; pointCloudToMesh(mesh1, aligned_cloud); pointCloudToMesh(mesh2, *target_cloud); mesh1.saveToPlyFile("debug_icp_1.ply"); mesh2.saveToPlyFile("debug_icp_2.ply"); } if (!reg.hasConverged()) { ntk_dbg(1) << "ICP did not converge, ignoring."; return false; } ntk_dbg_print(reg.getFitnessScore(), 1); Eigen::Matrix4f t = reg.getFinalTransformation (); cv::Mat1f T(4,4); //toOpencv(t,T); for (int r = 0; r < 4; ++r) for (int c = 0; c < 4; ++c) T(r,c) = t(r,c); relative_pose.setCameraTransform(T); return true; }
void RGBDCalibration::computeInfraredIntrinsicsFromDepth() { depth_intrinsics.copyTo(infrared_intrinsics); depth_distortion.copyTo(infrared_distortion); double& fx = infrared_intrinsics(0,0); double& fy = infrared_intrinsics(1,1); double& cx = infrared_intrinsics(0,2); double& cy = infrared_intrinsics(1,2); ntk_dbg(1) << cv::format("fx: %f fy: %f cx: %f cy: %f\n", fx, fy, cx, cy); cx = cx - infraredDepthOffsetX(); cy = cy - infraredDepthOffsetY() + 16; double ratio = double(infrared_size.width) / depth_size.width; ntk_dbg_print(ratio, 1); fx *= ratio; fy *= ratio; cx *= ratio; cy *= ratio; ntk_dbg(1) << cv::format("fx: %f fy: %f cx: %f cy: %f\n", fx, fy, cx, cy); }
void SiftHough :: houghpointsFromMatch(std::list<HoughPoint>& points, SiftPointMatchConstPtr match) { ntk_assert(match, "Null pointer."); const SiftPointTransform& point_transform = match->pointTransform(); std::vector<unsigned char> orientation_bins(2); std::vector<int> scale_bins(2); std::vector<int> x_bins(2); std::vector<int> y_bins(2); // orientation bins { unsigned max_bins = (unsigned) (std::floor((2.0*M_PI)/m_params.delta_orientation)-1); double o = point_transform.orientation / m_params.delta_orientation; orientation_bins[0] = (unsigned) std::floor(o); if ((o-orientation_bins[0]) < 0.5) { if (orientation_bins[0] == 0) orientation_bins[1] = max_bins; else orientation_bins[1] = orientation_bins[0] - 1; } else { if (orientation_bins[0] == max_bins) orientation_bins[1] = 0; else orientation_bins[1] = orientation_bins[0] + 1; } ntk_dbg_print(point_transform.orientation, 2); ntk_dbg_print(o, 2); ntk_dbg_print(orientation_bins[0], 2); ntk_dbg_print(orientation_bins[1], 2); } // scale bins { double s = log(point_transform.scale)/log(2.0); scale_bins[0] = (int)floor(s); int s2; if (s > scale_bins[0]+0.5) s2 = scale_bins[0] + 1; else s2 = scale_bins[0] - 1; scale_bins[1] = s2; } const VisualObjectView& model = match->pose()->modelView(); cv::Rect_<float> model_bbox = model.boundingRect(); const ObjectPose& model_pose = *match->pose(); // Vote for 16 bins in most cases. for (unsigned s = 0; s < scale_bins.size(); ++s) for (unsigned o = 0; o < orientation_bins.size(); ++o) { HoughPoint p; p.object_id = model.id(); p.orientation = orientation_bins[o]; p.scale = scale_bins[s]; double actual_scale = pow(2.0, p.scale); ntk_dbg_print(actual_scale, 2); // double max_dim = (model_bbox.width()*actual_scale + model_bbox.height()*actual_scale) / 2.0; double max_dim = std::max(model_bbox.width*actual_scale, model_bbox.height*actual_scale); max_dim *= m_params.delta_location; double x = model_pose.projectedCenter().x / max_dim; double y = model_pose.projectedCenter().y / max_dim; x_bins[0] = (int)floor(x); x_bins[1] = x_bins[0] + ((x-x_bins[0]) < 0.5 ? -1 : 1); y_bins[0] = (int)floor(y); y_bins[1] = y_bins[0] + ((y-y_bins[0]) < 0.5 ? -1 : 1); for (unsigned y = 0; y < y_bins.size(); ++y) for (unsigned x = 0; x < x_bins.size(); ++x) { p.x = x_bins[x]; p.y = y_bins[y]; points.push_back(p); } } }
bool RelativePoseEstimatorICPWithNormals<PointT> :: computeRegistration(Pose3D& relative_pose, PointCloudConstPtr source_cloud, PointCloudConstPtr target_cloud, PointCloudType& aligned_cloud) { #ifndef HAVE_PCL_GREATER_THAN_1_2_0 return super::computeRegistration(relative_pose, source_cloud, target_cloud, aligned_cloud); #else pcl::IterativeClosestPoint<PointT, PointT> reg; typedef pcl::registration::TransformationEstimationPointToPlaneLLS<PointT, PointT> PointToPlane; boost::shared_ptr<PointToPlane> point_to_plane (new PointToPlane); reg.setTransformationEstimation (point_to_plane); // typedef TransformationEstimationRGBD<PointT, PointT> TransformRGBD; // boost::shared_ptr<TransformRGBD> transform_rgbd (new TransformRGBD); // reg.setTransformationEstimation (transform_rgbd); #ifdef HAVE_PCL_GREATER_THAN_1_6_0 // rejectors are not well supported before 1.7 boost::shared_ptr<pcl::registration::CorrespondenceRejectorDistance> rejector_distance (new pcl::registration::CorrespondenceRejectorDistance); rejector_distance->setInputSource<PointT>(source_cloud); rejector_distance->setInputTarget<PointT>(target_cloud); rejector_distance->setMaximumDistance(m_distance_threshold); reg.addCorrespondenceRejector(rejector_distance); boost::shared_ptr<pcl::registration::CorrespondenceRejectorSurfaceNormal> rejector_normal (new pcl::registration::CorrespondenceRejectorSurfaceNormal); rejector_normal->setThreshold(cos(M_PI/4.f)); rejector_normal->initializeDataContainer<PointT, PointT>(); rejector_normal->setInputSource<PointT>(source_cloud); rejector_normal->setInputTarget<PointT>(target_cloud); rejector_normal->setInputNormals<PointT, PointT>(source_cloud); rejector_normal->setTargetNormals<PointT, PointT>(target_cloud); reg.addCorrespondenceRejector(rejector_normal); boost::shared_ptr<pcl::registration::CorrespondenceRejectorOneToOne> rejector_one_to_one (new pcl::registration::CorrespondenceRejectorOneToOne); reg.addCorrespondenceRejector(rejector_one_to_one); typedef pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> RejectorConsensusT; boost::shared_ptr<RejectorConsensusT> rejector_ransac (new RejectorConsensusT()); rejector_ransac->setInputSource(source_cloud); rejector_ransac->setInputTarget(target_cloud); rejector_ransac->setInlierThreshold(m_ransac_outlier_threshold); rejector_ransac->setMaxIterations(100); reg.addCorrespondenceRejector(rejector_ransac); boost::shared_ptr<pcl::registration::CorrespondenceRejectorVarTrimmed> rejector_var_trimmed (new pcl::registration::CorrespondenceRejectorVarTrimmed()); rejector_var_trimmed->setInputSource<PointT>(source_cloud); rejector_var_trimmed->setInputTarget<PointT>(target_cloud); rejector_var_trimmed->setMinRatio(0.1f); rejector_var_trimmed->setMaxRatio(0.75f); reg.addCorrespondenceRejector(rejector_var_trimmed); boost::shared_ptr<pcl::registration::CorrespondenceRejectorTrimmed> rejector_trimmed (new pcl::registration::CorrespondenceRejectorTrimmed()); rejector_trimmed->setMinCorrespondences(static_cast<int>(0.1f * source_cloud->size())); rejector_trimmed->setOverlapRatio(0.5f); reg.addCorrespondenceRejector(rejector_trimmed); #endif #if 0 ntk::Mesh target_mesh; pointCloudToMesh(target_mesh, *target_cloud); target_mesh.saveToPlyFile("debug_target.ply"); ntk::Mesh source_mesh; pointCloudToMesh(source_mesh, *source_cloud); source_mesh.saveToPlyFile("debug_source.ply"); #endif reg.setMaximumIterations (m_max_iterations); reg.setTransformationEpsilon (1e-10); reg.setMaxCorrespondenceDistance (m_distance_threshold); reg.setRANSACOutlierRejectionThreshold(m_ransac_outlier_threshold); #ifdef HAVE_PCL_GREATER_THAN_1_6_0 reg.setInputSource (source_cloud); #else reg.setInputCloud (source_cloud); #endif reg.setInputTarget (target_cloud); reg.align (aligned_cloud); if (!reg.hasConverged()) { ntk_dbg(1) << "ICP did not converge, ignoring."; return false; } if (0) { ntk::Mesh mesh1, mesh2; pointCloudToMesh(mesh1, aligned_cloud); pointCloudToMesh(mesh2, *target_cloud); mesh1.saveToPlyFile("debug_icp_1.ply"); mesh2.saveToPlyFile("debug_icp_2.ply"); } #if 0 ntk::Mesh debug_mesh; pointCloudToMesh(debug_mesh, aligned_cloud); debug_mesh.saveToPlyFile("debug_aligned.ply"); #endif ntk_dbg_print(reg.getFitnessScore(), 1); Eigen::Matrix4f t = reg.getFinalTransformation (); cv::Mat1f T(4,4); //toOpencv(t,T); for (int r = 0; r < 4; ++r) for (int c = 0; c < 4; ++c) T(r,c) = t(r,c); relative_pose.setCameraTransform(T); #endif return true; }
bool RelativePoseEstimatorICP :: estimateNewPose(const RGBDImage& image) { if (m_ref_cloud.points.size() < 1) { ntk_dbg(1) << "Reference cloud was empty"; return false; } PointCloud<PointXYZ>::Ptr target = m_ref_cloud.makeShared(); PointCloud<PointXYZ>::Ptr source (new PointCloud<PointXYZ>()); rgbdImageToPointCloud(*source, image); PointCloud<PointXYZ>::Ptr filtered_source (new PointCloud<PointXYZ>()); PointCloud<PointXYZ>::Ptr filtered_target (new PointCloud<PointXYZ>()); pcl::VoxelGrid<pcl::PointXYZ> grid; grid.setLeafSize (m_voxel_leaf_size, m_voxel_leaf_size, m_voxel_leaf_size); grid.setInputCloud(source); grid.filter(*filtered_source); grid.setInputCloud(target); grid.filter(*filtered_target); PointCloud<PointXYZ> cloud_reg; IterativeClosestPoint<PointXYZ, PointXYZ> reg; reg.setMaximumIterations (m_max_iterations); reg.setTransformationEpsilon (1e-5); reg.setMaxCorrespondenceDistance (m_distance_threshold); reg.setInputCloud (filtered_source); reg.setInputTarget (filtered_target); reg.align (cloud_reg); if (!reg.hasConverged()) { ntk_dbg(1) << "ICP did not converge, ignoring."; return false; } Eigen::Matrix4f t = reg.getFinalTransformation (); cv::Mat1f T(4,4); //toOpencv(t,T); for (int r = 0; r < 4; ++r) for (int c = 0; c < 4; ++c) T(r,c) = t(r,c); Pose3D icp_pose; icp_pose.setCameraTransform(T); ntk_dbg_print(icp_pose.cvTranslation(), 1); // Pose3D stores the transformation from 3D space to image. // So we have to invert everything so that unprojecting from // image to 3D gives the right transformation. // H2' = ICP * H1' m_current_pose.resetCameraTransform(); m_current_pose = *image.calibration()->depth_pose; m_current_pose.invert(); m_current_pose.applyTransformAfter(icp_pose); m_current_pose.invert(); return true; }