void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets) { Eigen::Vector3f x, y; x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]]; y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]]; Eigen::Matrix3f C; C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y; Eigen::Matrix3f IC = C.inverse(); for (int i = 0; i < 3; i++) { B(0, 2 * i + 0) = IC(1, i); B(0, 2 * i + 1) = 0.0f; B(1, 2 * i + 0) = 0.0f; B(1, 2 * i + 1) = IC(2, i); B(2, 2 * i + 0) = IC(2, i); B(2, 2 * i + 1) = IC(1, i); } Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * C.determinant() / 2.0f; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0)); Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1)); Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0)); Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1)); triplets.push_back(trplt11); triplets.push_back(trplt12); triplets.push_back(trplt21); triplets.push_back(trplt22); } } }
bool PositionBasedElasticRod::ComputeDarbouxVector(const Eigen::Matrix3f& dA, const Eigen::Matrix3f& dB, const float mid_edge_length, Eigen::Vector3f& darboux_vector) { float factor = 1.0f + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2)); factor = 2.0f / (mid_edge_length * factor); for (int c = 0; c < 3; ++c) { const int i = permutation[c][0]; const int j = permutation[c][1]; const int k = permutation[c][2]; darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j)); } darboux_vector *= factor; return true; }
void transformOrientationToGroundFrame(const VirtualRobot::RobotPtr& robot, const Eigen::Matrix6Xf& leftFootTrajectory, const VirtualRobot::RobotNodePtr& leftFoot, const VirtualRobot::RobotNodePtr& rightFoot, const VirtualRobot::RobotNodeSetPtr& bodyJoints, const Eigen::MatrixXf& bodyTrajectory, const Eigen::Matrix3Xf& trajectory, const std::vector<SupportInterval>& intervals, Eigen::Matrix3Xf& relativeTrajectory) { Eigen::Matrix4f leftInitialPose = bodyJoints->getKinematicRoot()->getGlobalPose(); int N = trajectory.cols(); int M = trajectory.rows(); relativeTrajectory.resize(M, N); BOOST_ASSERT(M > 0 && M <= 3); auto intervalIter = intervals.begin(); for (int i = 0; i < N; i++) { while (i >= intervalIter->endIdx) { intervalIter = std::next(intervalIter); } // Move basis along with the left foot Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity(); VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1), leftFootTrajectory.block(3, i, 3, 1), leftFootPose); robot->setGlobalPose(leftFootPose); bodyJoints->setJointValues(bodyTrajectory.col(i)); Eigen::Matrix3f worldToRef = computeGroundFrame(leftFoot->getGlobalPose(), rightFoot->getGlobalPose(), intervalIter->phase).block(0, 0, 3, 3); relativeTrajectory.block(0, i, M, 1) = worldToRef.colPivHouseholderQr().solve(trajectory.col(i)).block(0, 0, M, 1); } }
template <typename PointT> void pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD ( const pcl::PointCloud<PointT> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud<PointT> &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::VectorXf &transform) { transform.resize (16); Eigen::Vector4f centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (cloud_src, indices_src, centroid_src); compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt); // Subtract the centroids from source, target Eigen::MatrixXf cloud_src_demean; demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean); Eigen::MatrixXf cloud_tgt_demean; demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean); // Assemble the correlation matrix H = source * target' Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f u = svd.matrixU (); Eigen::Matrix3f v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int x = 0; x < 3; ++x) v (x, 2) *= -1; } Eigen::Matrix3f R = v * u.transpose (); // Return the correct transformation transform.segment<3> (0) = R.row (0); transform[12] = 0; transform.segment<3> (4) = R.row (1); transform[13] = 0; transform.segment<3> (8) = R.row (2); transform[14] = 0; Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> (); transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0; }
// ---------------------------------------------------------------------------------------------- bool PositionBasedElasticRod::ComputeMaterialFrame( const Eigen::Vector3f& pA, const Eigen::Vector3f& pB, const Eigen::Vector3f& pG, Eigen::Matrix3f& frame) { frame.col(2) = (pB - pA); frame.col(2).normalize(); frame.col(1) = (frame.col(2).cross(pG - pA)); frame.col(1).normalize(); frame.col(0) = frame.col(1).cross(frame.col(2)); // frame.col(0).normalize(); return true; }
bool FruitDetector::enhanceAlignment( const Eigen::Matrix3f& new_rotation, const boost::shared_ptr<Fruit>& f) { // get euler angles. (Z-Y-X) convention --> (Yaw, Pitch, Roll) const Eigen::Vector3f euler_angles = new_rotation.eulerAngles(2, 1, 0); const float new_yaw = euler_angles[0]; const float new_pitch = euler_angles[1]; const float new_roll = euler_angles[2]; const float old_roll = f->get_RPY().roll; const float old_pitch = f->get_RPY().pitch; // check if the new aligned model is less tilted if (new_roll < old_roll && new_pitch < old_pitch) return true; return false; }
bool ObjectFinder::find_toy_block(float surface_height, geometry_msgs::PoseStamped &object_pose) { Eigen::Vector3f plane_normal; double plane_dist; //bool valid_plane; Eigen::Vector3f major_axis; Eigen::Vector3f centroid; bool found_object = true; //should verify this double block_height = 0.035; //this height is specific to the TOY_BLOCK model //if insufficient points in plane, find_plane_fit returns "false" //should do more sanity testing on found_object status //hard-coded search bounds based on a block of width 0.035 found_object = pclUtils_.find_plane_fit(0.4, 1, -0.5, 0.5, surface_height + 0.025, surface_height + 0.045, 0.001, plane_normal, plane_dist, major_axis, centroid); //should have put a return value on find_plane_fit; // if (plane_normal(2) < 0) plane_normal(2) *= -1.0; //in world frame, normal must point UP Eigen::Matrix3f R; Eigen::Vector3f y_vec; R.col(0) = major_axis; R.col(2) = plane_normal; R.col(1) = plane_normal.cross(major_axis); Eigen::Quaternionf quat(R); object_pose.header.frame_id = "base_link"; object_pose.pose.position.x = centroid(0); object_pose.pose.position.y = centroid(1); //the TOY_BLOCK model has its origin in the middle of the block, not the top surface //so lower the block model origin by half the block height from upper surface object_pose.pose.position.z = centroid(2)-0.5*block_height; //create R from normal and major axis, then convert R to quaternion object_pose.pose.orientation.x = quat.x(); object_pose.pose.orientation.y = quat.y(); object_pose.pose.orientation.z = quat.z(); object_pose.pose.orientation.w = quat.w(); return found_object; }
std::vector<Point3f> PnPUtil::BackprojectPts(const std::vector<Point2f>& pts, const Eigen::Matrix4f& camTf, const Eigen::Matrix3f& K, const Mat& depth) { std::vector<Point3f> pts3d; for(std::size_t i = 0; i < pts.size(); i++) { Point2f pt = pts[i]; Eigen::Vector3f hkp(pt.x, pt.y, 1); Eigen::Vector3f backproj = K.inverse()*hkp; backproj /= backproj(2); backproj *= depth.at<float>(pt.y, pt.x); if(depth.at<float>(pt.y, pt.x) == 0) std::cout << "Bad depth (0)" << std::endl; Eigen::Vector4f backproj_h(backproj(0), backproj(1), backproj(2), 1); backproj_h = camTf*backproj_h; pts3d.push_back(Point3f(backproj_h(0), backproj_h(1), backproj_h(2))); } return pts3d; }
void SQ_fitter<PointT>::getBoundingBox(const PointCloudPtr &_cloud, double _dim[3], double _trans[3], double _rot[3] ) { // 1. Compute the bounding box center Eigen::Vector4d centroid; pcl::compute3DCentroid( *_cloud, centroid ); _trans[0] = centroid(0); _trans[1] = centroid(1); _trans[2] = centroid(2); // 2. Compute main axis orientations pcl::PCA<PointT> pca; pca.setInputCloud( _cloud ); Eigen::Vector3f eigVal = pca.getEigenValues(); Eigen::Matrix3f eigVec = pca.getEigenVectors(); // Make sure 3 vectors are normal w.r.t. each other Eigen::Vector3f temp; eigVec.col(2) = eigVec.col(0); // Z Eigen::Vector3f v3 = (eigVec.col(1)).cross( eigVec.col(2) ); eigVec.col(0) = v3; Eigen::Vector3f rpy = eigVec.eulerAngles(2,1,0); _rot[0] = (double)rpy(2); _rot[1] = (double)rpy(1); _rot[2] = (double)rpy(0); // Transform _cloud Eigen::Matrix4f transf = Eigen::Matrix4f::Identity(); transf.block(0,3,3,1) << (float)centroid(0), (float)centroid(1), (float)centroid(2); transf.block(0,0,3,3) = eigVec; Eigen::Matrix4f tinv; tinv = transf.inverse(); PointCloudPtr cloud_temp( new pcl::PointCloud<PointT>() ); pcl::transformPointCloud( *_cloud, *cloud_temp, tinv ); // Get maximum and minimum PointT minPt; PointT maxPt; pcl::getMinMax3D( *cloud_temp, minPt, maxPt ); _dim[0] = ( maxPt.x - minPt.x ) / 2.0; _dim[1] = ( maxPt.y - minPt.y ) / 2.0; _dim[2] = ( maxPt.z - minPt.z ) / 2.0; }
pcl::PointCloud<pcl::PointXYZRGB> createPointCloud( const Eigen::Matrix3f K, const cv::Mat &depth, const cv::Mat &color ) { pcl::PointCloud<pcl::PointXYZRGB> pointCloud; int w = depth.cols; int h = depth.rows; float *pDepth = (float*)depth.data; float *pColor = (float*)color.data; for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { size_t off = (y*w + x); size_t off3 = off * 3; // Check depth value validity float d = pDepth[x + y*w]; if (d == 0.0f || std::isnan(d)) { continue; } // RGBXYZ point pcl::PointXYZRGB point; // Color point.b = max(0, min(255, (int)(pColor[off3 ]*255))); point.g = max(0, min(255, (int)(pColor[off3+1]*255))); point.r = max(0, min(255, (int)(pColor[off3+2]*255))); // Position Eigen::Vector3f pos = K.inverse() * Eigen::Vector3f(x*d, y*d, d); point.x = pos[0]; point.y = pos[1]; point.z = pos[2]; pointCloud.push_back(point); } } return pointCloud; }
void computeSensorOffsetAndK(Eigen::Isometry3f &sensorOffset, Eigen::Matrix3f &cameraMatrix, ParameterCamera *cameraParam, int reduction) { sensorOffset = Isometry3f::Identity(); cameraMatrix.setZero(); int cmax = 4; int rmax = 3; for (int c=0; c<cmax; c++){ for (int r=0; r<rmax; r++){ sensorOffset.matrix()(r, c) = cameraParam->offset()(r, c); if (c<3) cameraMatrix(r,c) = cameraParam->Kcam()(r, c); } } sensorOffset.translation() = Vector3f(0.15f, 0.0f, 0.05f); Quaternionf quat = Quaternionf(0.5, -0.5, 0.5, -0.5); sensorOffset.linear() = quat.toRotationMatrix(); sensorOffset.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f; float scale = 1./reduction; cameraMatrix *= scale; cameraMatrix(2,2) = 1; }
/** \brief Computes and sets the multilevel Shi-Tomasi Score \ref s_, considering a defined pyramid level interval. * * @param l1 - Start level (l1<l2) * @param l2 - End level (l1<l2) */ void computeMultilevelShiTomasiScore(const int l1 = 0, const int l2 = nLevels_-1) const{ H_.setZero(); int count = 0; for(int i=l1;i<=l2;i++){ if(isValidPatch_[i]){ H_ += pow(0.25,i)*patches_[i].getHessian(); count++; } } if(count > 0){ const float dXX = H_(0,0)/(count*patchSize*patchSize); const float dYY = H_(1,1)/(count*patchSize*patchSize); const float dXY = H_(0,1)/(count*patchSize*patchSize); e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY))); e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY))); s_ = e0_; } else { e0_ = 0; e1_ = 0; s_ = -1; } }
/** * ComputeCovarianceMatrix */ void ZAdaptiveNormals::computeCovarianceMatrix (const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov) { Eigen::Vector3f pt; cov.setZero (); for (unsigned i = 0; i < indices.size (); ++i) { pt = cloud.data[indices[i]] - mean; cov(1,1) += pt[1] * pt[1]; cov(1,2) += pt[1] * pt[2]; cov(2,2) += pt[2] * pt[2]; pt *= pt[0]; cov(0,0) += pt[0]; cov(0,1) += pt[1]; cov(0,2) += pt[2]; } cov(1,0) = cov(0,1); cov(2,0) = cov(0,2); cov(2,1) = cov(1,2); }
/** * computeCovarianceMatrix */ void PlaneEstimationRANSAC::computeCovarianceMatrix (const std::vector<Eigen::Vector3f> &pts, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov) { Eigen::Vector3f pt; cov.setZero (); for (unsigned i = 0; i < indices.size (); ++i) { pt = pts[indices[i]] - mean; cov(1,1) += pt[1] * pt[1]; cov(1,2) += pt[1] * pt[2]; cov(2,2) += pt[2] * pt[2]; pt *= pt[0]; cov(0,0) += pt[0]; cov(0,1) += pt[1]; cov(0,2) += pt[2]; } cov(1,0) = cov(0,1); cov(2,0) = cov(0,2); cov(2,1) = cov(1,2); }
int main(int argc, char **argv){ #if 0 DOF6::TFLinkvf rot1,rot2; Eigen::Matrix4f tf1 = build_random_tflink(rot1,3); Eigen::Matrix4f tf2 = build_random_tflink(rot2,3); DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::TFLinkvf,float> abc(rot1.makeShared(), rot2.makeShared()); abc.getRotation(); abc.getTranslation(); std::cout<<"tf1\n"<<tf1<<"\n"; std::cout<<"tf2\n"<<tf2<<"\n"; std::cout<<"tf1*2\n"<<tf1*tf2<<"\n"; std::cout<<"tf2*1\n"<<tf2*tf1<<"\n"; std::cout<<"tf1\n"<<rot1.getTransformation()<<"\n"; std::cout<<"tf2\n"<<rot2.getTransformation()<<"\n"; std::cout<<"tf1*2\n"<<(rot1+rot2).getTransformation()<<"\n"; rot1.check(); rot2.check(); return 0; pcl::RotationFromCorrespondences rfc; Eigen::Vector3f n, nn, v,n2,n3,z,y,tv; float a = 0.1f; z.fill(0);y.fill(0); z(2)=1;y(1)=1; nn.fill(0); nn(0) = 1; Eigen::AngleAxisf aa(a,nn); nn.fill(100); n.fill(0); n(0) = 1; n2.fill(0); n2=n; n2(1) = 0.2; n3.fill(0); n3=n; n3(2) = 0.2; n2.normalize(); n3.normalize(); tv.fill(1); tv.normalize(); #if 0 #if 0 rfc.add(n,aa.toRotationMatrix()*n+nn, 1*n.cross(y),1*n.cross(z), 1*(aa.toRotationMatrix()*n).cross(y),1*(aa.toRotationMatrix()*n).cross(z), 1,1/sqrtf(3)); #else rfc.add(n,aa.toRotationMatrix()*n, 0*n.cross(y),0*n.cross(z), 0*(aa.toRotationMatrix()*n).cross(y),0*(aa.toRotationMatrix()*n).cross(z), 1,0); #endif #if 1 rfc.add(n2,aa.toRotationMatrix()*n2+nn, tv,tv, tv,tv, 1,1); #else rfc.add(n2,aa.toRotationMatrix()*n2+nn, 1*n2.cross(y),1*n2.cross(z), 1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z), 1,1/sqrtf(3)); #endif #else float f=1; Eigen::Vector3f cyl; cyl.fill(1); cyl(0)=1; Eigen::Matrix3f cylM; cylM.fill(0); cylM.diagonal() = cyl; rfc.add(n,aa.toRotationMatrix()*n, f*n.cross(y),f*n.cross(z), f*(aa.toRotationMatrix()*n).cross(y),f*(aa.toRotationMatrix()*n).cross(z), 1,0); rfc.add(n2,aa.toRotationMatrix()*n2+nn, 1*n2.cross(y),1*n2.cross(z), 1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z), 1,1); #endif rfc.add(n3,aa.toRotationMatrix()*n3+nn, //tv,tv, //tv,tv, n3.cross(y),n3.cross(z), 1*(aa.toRotationMatrix()*n3).cross(y),1*(aa.toRotationMatrix()*n3).cross(z), 1,1); std::cout<<"comp matrix:\n"<<rfc.getTransformation()<<"\n\n"; std::cout<<"real matrix:\n"<<aa.toRotationMatrix()<<"\n\n"; return 0; //rfc.covariance_.normalize(); rfc.covariance_ = (rfc.var_*rfc.covariance_.inverse().transpose()*rfc.covariance_); std::cout<<"comp matrix: "<<rfc.getTransformation()<<"\n\n"; std::cout<<"real matrix: "<<aa.toRotationMatrix()*aa.toRotationMatrix()<<"\n\n"; return 0; #endif testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }
void onGMM(const gaussian_mixture_model::GaussianMixture & mix) { visualization_msgs::MarkerArray msg; ROS_INFO("gmm_rviz_converter: Received message."); uint i; for (i = 0; i < mix.gaussians.size(); i++) { visualization_msgs::Marker marker; marker.header.frame_id = m_frame_id; marker.header.stamp = ros::Time::now(); marker.ns = m_rviz_namespace; marker.id = i; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.lifetime = ros::Duration(); Eigen::Vector3f coords; for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++) coords[ir] = m_conversion_mask[ir]->GetMean(m_conversion_mask,mix.gaussians[i]); marker.pose.position.x = coords.x(); marker.pose.position.y = coords.y(); marker.pose.position.z = coords.z(); Eigen::Matrix3f covmat; for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++) for (uint ic = 0; ic < NUM_OUTPUT_COORDINATES; ic++) covmat(ir,ic) = m_conversion_mask[ir]->GetCov(m_conversion_mask,mix.gaussians[i],ic); Eigen::EigenSolver<Eigen::Matrix3f> evsolver(covmat); Eigen::Matrix3f eigenvectors = evsolver.eigenvectors().real(); if (eigenvectors.determinant() < 0.0) eigenvectors.col(0) = - eigenvectors.col(0); Eigen::Matrix3f rotation = eigenvectors; Eigen::Quaternionf quat = Eigen::Quaternionf(Eigen::AngleAxisf(rotation)); marker.pose.orientation.x = quat.x(); marker.pose.orientation.y = quat.y(); marker.pose.orientation.z = quat.z(); marker.pose.orientation.w = quat.w(); Eigen::Vector3f eigenvalues = evsolver.eigenvalues().real(); Eigen::Vector3f scale = Eigen::Vector3f(eigenvalues.array().abs().sqrt()); if (m_normalize) scale.normalize(); marker.scale.x = mix.weights[i] * scale.x() * m_scale; marker.scale.y = mix.weights[i] * scale.y() * m_scale; marker.scale.z = mix.weights[i] * scale.z() * m_scale; marker.color.a = 1.0; rainbow(float(i) / float(mix.gaussians.size()),marker.color.r,marker.color.g,marker.color.b); msg.markers.push_back(marker); } // this a waste of resources, but we need to delete old markers in some way // someone should add a "clear all" command to rviz // (using expiration time is not an option, here) for ( ; i < m_max_markers; i++) { visualization_msgs::Marker marker; marker.id = i; marker.action = visualization_msgs::Marker::DELETE; marker.lifetime = ros::Duration(); marker.header.frame_id = m_frame_id; marker.header.stamp = ros::Time::now(); marker.ns = m_rviz_namespace; msg.markers.push_back(marker); } m_pub.publish(msg); ROS_INFO("gmm_rviz_converter: Sent message."); }
osgpcl::PointCloudGeometry* osgpcl::SurfelFactoryFF<PointT, NormalT, RadiusT>::buildGeometry( bool unique_state) { typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>(); typename pcl::PointCloud<PointT>::ConstPtr xyz = this->getInputCloud<PointT>(); typename pcl::PointCloud<RadiusT>::ConstPtr rads = this->getInputCloud<RadiusT>(); if (xyz ==NULL) return NULL; if (normals ==NULL) return NULL; if (rads ==NULL) return NULL; if (xyz->points.size() != normals->points.size()) return NULL; if (rads->points.size() != normals->points.size()) return NULL; pcl::IndicesConstPtr indices = indices_; { bool rebuild_indices= false; if (indices_ == NULL) rebuild_indices=true; else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true; if (rebuild_indices){ pcl::IndicesPtr idxs(new std::vector<int>); idxs->reserve(xyz->points.size()); for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i); indices= idxs; } } osg::Vec3Array* pts = new osg::Vec3Array; osg::Vec3Array* npts = new osg::Vec3Array; int fan_size = 1+ circle_cache.rows(); pts->reserve(indices->size()*fan_size ); npts->reserve(indices->size()*fan_size); osg::Geometry* geom = new osg::Geometry; for(int i=0, pstart=0; i<indices->size(); i++){ const int& idx = (*indices)[i]; const PointT& pt = xyz->points[idx]; const NormalT& npt = normals->points[idx]; pts->push_back(osg::Vec3(pt.x, pt.y, pt.z)); npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z)); pcl::Normal nt; Eigen::Matrix3f rot = Eigen::Matrix3f::Identity(); rot.row(2) << npt.getNormalVector3fMap().transpose(); Eigen::Vector3f ax2(1,0,0); if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) { ax2 << 0,1,0; if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) { ax2 << 0,0,1; } } rot.row(1) << ax2.cross(npt.getNormalVector3fMap()).normalized().transpose(); rot.row(0) = ( ax2 - ax2.dot(npt.getNormalVector3fMap() )*npt.getNormalVector3fMap() ).normalized(); rot = rot*rads->points[idx].radius; for(int j=0; j<circle_cache.rows(); j++){ Eigen::Vector3f apt = rot*circle_cache.row(j).transpose() + pt.getVector3fMap(); pts->push_back(osg::Vec3(apt[0],apt[1],apt[2])); npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z)); } geom->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, pstart, fan_size ) ); pstart+= fan_size; } geom->setVertexArray( pts ); geom->setNormalArray(npts); geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX ); geom->setStateSet(stateset_); return geom; }
/// /// @par Detailed description /// ... /// @param [in, out] (param1) ... /// @return ... /// @note ... Eigen::Matrix3f sasmath::Math:: find_u(sasmol::SasMol &mol, int &frame) { Eigen::Matrix3f r ; float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ; float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ; float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ; float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ; float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ; float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ; float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ; float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ; float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ; r << rxx,rxy,rxz, ryx,ryy,ryz, rzx,rzy,rzz; Eigen::Matrix3f rtr = r.transpose() * r ; Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr); if (eigensolver.info() != Eigen::Success) { std::cout << "find_u calculation failed" << std::endl ; } Eigen::Matrix<float,3,1> uk ; // eigenvalues Eigen::Matrix3f ak ; // eigenvectors uk = eigensolver.eigenvalues() ; ak = eigensolver.eigenvectors() ; Eigen::Matrix3f akt = ak.transpose() ; Eigen::Matrix3f new_ak ; new_ak.row(0) = akt.row(2) ; //sort eigenvectors new_ak.row(1) = akt.row(1) ; new_ak.row(2) = akt.row(0) ; // python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0] //Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ; Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ; Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ; Eigen::Matrix<float,3,1> urak0 ; if(uk[2] == 0.0) { urak0 = 1E15 * rak0 ; } else { urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ; } Eigen::Matrix<float,3,1> urak1 ; if(uk[1] == 0.0) { urak1 = 1E15 * rak1 ; } else { urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ; } Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ; Eigen::Matrix3f bk ; bk << urak0,urak1,urak2; Eigen::Matrix3f u ; u = (bk * new_ak).transpose() ; return u ; /* u = array([[-0.94513198, 0.31068658, 0.100992 ], [-0.3006246 , -0.70612572, -0.64110165], [-0.12786863, -0.63628635, 0.76078203]]) check: u = -0.945133 0.310686 0.100992 -0.300624 -0.706126 -0.641102 -0.127869 -0.636287 0.760782 */ }
void LocalRecognitionPipeline<PointT>::correspondenceGrouping () { if(cg_algorithm_->getRequiresNormals() && (!scene_normals_ || scene_normals_->points.size() != scene_->points.size())) computeNormals<PointT>(scene_, scene_normals_, param_.normal_computation_method_); typename std::map<std::string, ObjectHypothesis<PointT> >::iterator it; for (it = obj_hypotheses_.begin (); it != obj_hypotheses_.end (); it++) { ObjectHypothesis<PointT> &oh = it->second; if(oh.model_scene_corresp_.size() < 3) continue; std::vector < pcl::Correspondences > corresp_clusters; cg_algorithm_->setSceneCloud (scene_); cg_algorithm_->setInputCloud (oh.model_->keypoints_); if(cg_algorithm_->getRequiresNormals()) cg_algorithm_->setInputAndSceneNormals(oh.model_->kp_normals_, scene_normals_); //we need to pass the keypoints_pointcloud and the specific object hypothesis cg_algorithm_->setModelSceneCorrespondences (oh.model_scene_corresp_); cg_algorithm_->cluster (corresp_clusters); std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms (corresp_clusters.size()); typename pcl::registration::TransformationEstimationSVD < PointT, PointT > t_est; for (size_t i = 0; i < corresp_clusters.size(); i++) t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, corresp_clusters[i], new_transforms[i]); if(param_.merge_close_hypotheses_) { std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > merged_transforms (corresp_clusters.size()); std::vector<bool> cluster_has_been_taken(corresp_clusters.size(), false); const double angle_thresh_rad = param_.merge_close_hypotheses_angle_ * M_PI / 180.f ; size_t kept=0; for (size_t i = 0; i < new_transforms.size(); i++) { if (cluster_has_been_taken[i]) continue; cluster_has_been_taken[i] = true; const Eigen::Vector3f centroid1 = new_transforms[i].block<3, 1> (0, 3); const Eigen::Matrix3f rot1 = new_transforms[i].block<3, 3> (0, 0); pcl::Correspondences merged_corrs = corresp_clusters[i]; for(size_t j=i; j < new_transforms.size(); j++) { const Eigen::Vector3f centroid2 = new_transforms[j].block<3, 1> (0, 3); const Eigen::Matrix3f rot2 = new_transforms[j].block<3, 3> (0, 0); const Eigen::Matrix3f rot_diff = rot2 * rot1.transpose(); double rotx = atan2(rot_diff(2,1), rot_diff(2,2)); double roty = atan2(-rot_diff(2,0), sqrt(rot_diff(2,1) * rot_diff(2,1) + rot_diff(2,2) * rot_diff(2,2))); double rotz = atan2(rot_diff(1,0), rot_diff(0,0)); double dist = (centroid1 - centroid2).norm(); if ( (dist < param_.merge_close_hypotheses_dist_) && (rotx < angle_thresh_rad) && (roty < angle_thresh_rad) && (rotz < angle_thresh_rad) ) { merged_corrs.insert( merged_corrs.end(), corresp_clusters[j].begin(), corresp_clusters[j].end() ); cluster_has_been_taken[j] = true; } } t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, merged_corrs, merged_transforms[kept]); kept++; } merged_transforms.resize(kept); new_transforms = merged_transforms; } std::cout << "Merged " << corresp_clusters.size() << " clusters into " << new_transforms.size() << " clusters. Total correspondences: " << oh.model_scene_corresp_.size () << " " << it->first << std::endl; // oh.visualize(*scene_); size_t existing_hypotheses = models_.size(); models_.resize( existing_hypotheses + new_transforms.size(), oh.model_ ); transforms_.insert(transforms_.end(), new_transforms.begin(), new_transforms.end()); } }
template<template<class > class Distance, typename PointT, typename FeatureT> bool GlobalNNPipelineROS<Distance,PointT,FeatureT>::classifyROS (classifier_srv_definitions::classify::Request & req, classifier_srv_definitions::classify::Response & response) { typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>()); pcl::fromROSMsg(req.cloud, *cloud); this->input_ = cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr pClusteredPCl (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::copyPointCloud(*cloud, *pClusteredPCl); //clear all data from previous visualization steps and publish empty markers/point cloud for (size_t i=0; i < markerArray_.markers.size(); i++) { markerArray_.markers[i].action = visualization_msgs::Marker::DELETE; } vis_pub_.publish( markerArray_ ); sensor_msgs::PointCloud2 scenePc2; vis_pc_pub_.publish(scenePc2); markerArray_.markers.clear(); for(size_t cluster_id=0; cluster_id < req.clusters_indices.size(); cluster_id++) { std::vector<int> cluster_indices_int; Eigen::Vector4f centroid; Eigen::Matrix3f covariance_matrix; const float r = std::rand() % 255; const float g = std::rand() % 255; const float b = std::rand() % 255; this->indices_.resize(req.clusters_indices[cluster_id].data.size()); for(size_t kk=0; kk < req.clusters_indices[cluster_id].data.size(); kk++) { this->indices_[kk] = static_cast<int>(req.clusters_indices[cluster_id].data[kk]); pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).r = 0.8*r; pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).g = 0.8*g; pClusteredPCl->at(req.clusters_indices[cluster_id].data[kk]).b = 0.8*b; } this->classify (); std::cout << "for cluster " << cluster_id << " with size " << cluster_indices_int.size() << ", I have following hypotheses: " << std::endl; object_perception_msgs::classification class_tmp; for(size_t kk=0; kk < this->categories_.size(); kk++) { std::cout << this->categories_[kk] << " with confidence " << this->confidences_[kk] << std::endl; std_msgs::String str_tmp; str_tmp.data = this->categories_[kk]; class_tmp.class_type.push_back(str_tmp); class_tmp.confidence.push_back( this->confidences_[kk] ); } response.class_results.push_back(class_tmp); typename pcl::PointCloud<PointT>::Ptr pClusterPCl_transformed (new pcl::PointCloud<PointT>()); pcl::computeMeanAndCovarianceMatrix(*this->input_, cluster_indices_int, covariance_matrix, centroid); Eigen::Matrix3f eigvects; Eigen::Vector3f eigvals; pcl::eigen33(covariance_matrix, eigvects, eigvals); Eigen::Vector3f centroid_transformed = eigvects.transpose() * centroid.topRows(3); Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero(4,4); transformation_matrix.block<3,3>(0,0) = eigvects.transpose(); transformation_matrix.block<3,1>(0,3) = -centroid_transformed; transformation_matrix(3,3) = 1; pcl::transformPointCloud(*this->input_, cluster_indices_int, *pClusterPCl_transformed, transformation_matrix); //pcl::transformPointCloud(*frame_, cluster_indices_int, *frame_eigencoordinates_, eigvects); PointT min_pt, max_pt; pcl::getMinMax3D(*pClusterPCl_transformed, min_pt, max_pt); std::cout << "Elongations along eigenvectors: " << max_pt.x - min_pt.x << ", " << max_pt.y - min_pt.y << ", " << max_pt.z - min_pt.z << std::endl; geometry_msgs::Point32 centroid_ros; centroid_ros.x = centroid[0]; centroid_ros.y = centroid[1]; centroid_ros.z = centroid[2]; response.centroid.push_back(centroid_ros); // calculating the bounding box of the cluster Eigen::Vector4f min; Eigen::Vector4f max; pcl::getMinMax3D (*this->input_, cluster_indices_int, min, max); object_perception_msgs::BBox bbox; geometry_msgs::Point32 pt; pt.x = min[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = min[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt); pt.x = min[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = min[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = min[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = min[1]; pt.z = max[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = max[1]; pt.z = min[2]; bbox.point.push_back(pt); pt.x = max[0]; pt.y = max[1]; pt.z = max[2]; bbox.point.push_back(pt); response.bbox.push_back(bbox); // getting the point cloud of the cluster typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>); pcl::copyPointCloud(*this->input_, cluster_indices_int, *cluster); sensor_msgs::PointCloud2 pc2; pcl::toROSMsg (*cluster, pc2); response.cloud.push_back(pc2); // visualize the result as ROS topic visualization_msgs::Marker marker; marker.header.frame_id = camera_frame_; marker.header.stamp = ros::Time::now(); //marker.header.seq = ++marker_seq_; marker.ns = "object_classification"; marker.id = cluster_id; marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::Marker::ADD; marker.pose.position.x = centroid_ros.x; marker.pose.position.y = centroid_ros.y - 0.1; marker.pose.position.z = centroid_ros.z - 0.1; marker.pose.orientation.x = 0; marker.pose.orientation.y = 0; marker.pose.orientation.z = 0; marker.pose.orientation.w = 1.0; marker.scale.z = 0.1; marker.color.a = 1.0; marker.color.r = r/255.f; marker.color.g = g/255.f; marker.color.b = b/255.f; std::stringstream marker_text; marker_text.precision(2); marker_text << this->categories_[0] << this->confidences_[0]; marker.text = marker_text.str(); markerArray_.markers.push_back(marker); } pcl::toROSMsg (*pClusteredPCl, scenePc2); vis_pc_pub_.publish(scenePc2); vis_pub_.publish( markerArray_ ); response.clusters_indices = req.clusters_indices; return true; }
) { Approx approx = Approx::custom().epsilon( 0.00001 ); OSinput oi = OSinput(pdg2014::Mb, 80.399, 91.1876, 125.6, 173.5); SECTION( "MS mass from OS one,\\mu=M" ) { // We use b-quark due to nl=2*NL bb<OS> dMt(oi, oi.MMb()); SECTION( "Three-loop zm3 = c0 + c1*nl + c2*nl^2" ) { Eigen::Matrix3f mM3; Eigen::Vector3f x3NL; x3NL(0) = dMt.x03(0,1); x3NL(1) = dMt.x03(1,1); x3NL(2) = dMt.x03(2,1); // c0 + c1*nl + c2*nl^2 mM3 << 1, 0, 0, 1, 2, 4, 1, 4, 16; Eigen::Vector3f ci3 = pow(4,-3)*mM3.colPivHouseholderQr().solve(x3NL); // NL^0
template<typename PointInT, typename PointNT, typename PointOutT> bool pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid, PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations, PointInTPtr & grid, pcl::PointIndices & indices) { Eigen::Vector3f plane_normal; plane_normal[0] = -centroid[0]; plane_normal[1] = -centroid[1]; plane_normal[2] = -centroid[2]; Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); plane_normal.normalize (); Eigen::Vector3f axis = plane_normal.cross (z_vector); double rotation = -asin (axis.norm ()); axis.normalize (); Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis)); grid->points.resize (processed->points.size ()); for (size_t k = 0; k < processed->points.size (); k++) grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap (); pcl::transformPointCloud (*grid, *grid, transformPC); Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0); Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0); centroid4f = transformPC * centroid4f; normal_centroid4f = transformPC * normal_centroid4f; Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]); Eigen::Vector4f farthest_away; pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away); farthest_away[3] = 0; float max_dist = (farthest_away - centroid4f).norm (); pcl::demeanPointCloud (*grid, centroid4f, *grid); Eigen::Matrix4f center_mat; center_mat.setIdentity (4, 4); center_mat (0, 3) = -centroid4f[0]; center_mat (1, 3) = -centroid4f[1]; center_mat (2, 3) = -centroid4f[2]; Eigen::Matrix3f scatter; scatter.setZero (); float sum_w = 0.f; //for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++) for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++) { Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap (); float d_k = (pvector).norm (); float w = (max_dist - d_k); Eigen::Vector3f diff = (pvector); Eigen::Matrix3f mat = diff * diff.transpose (); scatter = scatter + mat * w; sum_w += w; } scatter /= sum_w; Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV); Eigen::Vector3f evx = svd.matrixV ().col (0); Eigen::Vector3f evy = svd.matrixV ().col (1); Eigen::Vector3f evz = svd.matrixV ().col (2); Eigen::Vector3f evxminus = evx * -1; Eigen::Vector3f evyminus = evy * -1; Eigen::Vector3f evzminus = evz * -1; float s_xplus, s_xminus, s_yplus, s_yminus; s_xplus = s_xminus = s_yplus = s_yminus = 0.f; //disambiguate rf using all points for (int k = 0; k < static_cast<int> (grid->points.size ()); k++) { Eigen::Vector3f pvector = grid->points[k].getVector3fMap (); float dist_x, dist_y; dist_x = std::abs (evx.dot (pvector)); dist_y = std::abs (evy.dot (pvector)); if ((pvector).dot (evx) >= 0) s_xplus += dist_x; else s_xminus += dist_x; if ((pvector).dot (evy) >= 0) s_yplus += dist_y; else s_yminus += dist_y; } if (s_xplus < s_xminus) evx = evxminus; if (s_yplus < s_yminus) evy = evyminus; //select the axis that could be disambiguated more easily float fx, fy; float max_x = static_cast<float> (std::max (s_xplus, s_xminus)); float min_x = static_cast<float> (std::min (s_xplus, s_xminus)); float max_y = static_cast<float> (std::max (s_yplus, s_yminus)); float min_y = static_cast<float> (std::min (s_yplus, s_yminus)); fx = (min_x / max_x); fy = (min_y / max_y); Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]); if (normal3f.dot (evz) < 0) evz = evzminus; //if fx/y close to 1, it was hard to disambiguate //what if both are equally easy or difficult to disambiguate, namely fy == fx or very close float max_axis = std::max (fx, fy); float min_axis = std::min (fx, fy); if ((min_axis / max_axis) > axis_ratio_) { PCL_WARN("Both axis are equally easy/difficult to disambiguate\n"); Eigen::Vector3f evy_copy = evy; Eigen::Vector3f evxminus = evx * -1; Eigen::Vector3f evyminus = evy * -1; if (min_axis > min_axis_value_) { //combination of all possibilities evy = evx.cross (evz); Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); evx = evxminus; evy = evx.cross (evz); trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); evx = evy_copy; evy = evx.cross (evz); trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); evx = evyminus; evy = evx.cross (evz); trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); } else { //1-st case (evx selected) evy = evx.cross (evz); Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); //2-nd case (evy selected) evx = evy_copy; evy = evx.cross (evz); trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); } } else { if (fy < fx) { evx = evy; fx = fy; } evy = evx.cross (evz); Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); transformations.push_back (trans); } return true; }
// Eigen types to GLM types glm::mat3 eigen_matrix3f_to_glm_mat3(const Eigen::Matrix3f &m) { return glm::make_mat3x3((const float *)m.data()); }
void NMBasedCloudIntegration<PointT>::collectInfo () { size_t total_point_count = 0; for(size_t i = 0; i < input_clouds_.size(); i++) total_point_count += (indices_.empty() || indices_[i].empty()) ? input_clouds_[i]->size() : indices_[i].size(); VLOG(1) << "Allocating memory for point information of " << total_point_count << "points. "; big_cloud_info_.resize(total_point_count); std::vector<pcl::PointCloud<PointT> > input_clouds_aligned (input_clouds_.size()); std::vector<pcl::PointCloud<pcl::Normal> > input_normals_aligned (input_clouds_.size()); #pragma omp parallel for schedule(dynamic) for(size_t i=0; i < input_clouds_.size(); i++) { pcl::transformPointCloud(*input_clouds_[i], input_clouds_aligned[i], transformations_to_global_[i]); transformNormals(*input_normals_[i], input_normals_aligned[i], transformations_to_global_[i]); } size_t point_count = 0; for(size_t i=0; i < input_clouds_.size(); i++) { const pcl::PointCloud<PointT> &cloud_aligned = input_clouds_aligned[i]; const pcl::PointCloud<pcl::Normal> &normals_aligned = input_normals_aligned[i]; size_t kept_new_pts = 0; if (indices_.empty() || indices_[i].empty()) { for(size_t jj=0; jj<cloud_aligned.points.size(); jj++) { if ( !pcl::isFinite(cloud_aligned.points[jj]) || !pcl::isFinite(normals_aligned.points[jj]) ) continue; PointInfo &pt = big_cloud_info_[point_count + kept_new_pts]; pt.pt = cloud_aligned.points[jj]; pt.normal = normals_aligned.points[jj]; pt.sigma_lateral = pt_properties_[i][jj][0]; pt.sigma_axial = pt_properties_[i][jj][1]; pt.distance_to_depth_discontinuity = pt_properties_[i][jj][2]; pt.pt_idx = jj; kept_new_pts++; } } else { for(int idx : indices_[i]) { if ( !pcl::isFinite(cloud_aligned.points[idx]) || !pcl::isFinite(normals_aligned.points[idx]) ) continue; PointInfo &pt = big_cloud_info_[point_count + kept_new_pts]; pt.pt = cloud_aligned.points[idx]; pt.normal = normals_aligned.points[ idx ]; pt.sigma_lateral = pt_properties_[i][idx][0]; pt.sigma_axial = pt_properties_[i][idx][1]; pt.distance_to_depth_discontinuity = pt_properties_[i][idx][2]; pt.pt_idx = idx; kept_new_pts++; } } // compute and store remaining information #pragma omp parallel for schedule (dynamic) firstprivate(i, point_count, kept_new_pts) for(size_t jj=0; jj<kept_new_pts; jj++) { PointInfo &pt = big_cloud_info_ [point_count + jj]; pt.origin = i; Eigen::Matrix3f sigma = Eigen::Matrix3f::Zero(), sigma_aligned = Eigen::Matrix3f::Zero(); sigma(0,0) = pt.sigma_lateral; sigma(1,1) = pt.sigma_lateral; sigma(2,2) = pt.sigma_axial; const Eigen::Matrix4f &tf = transformations_to_global_[ i ]; Eigen::Matrix3f rotation = tf.block<3,3>(0,0); // or inverse? sigma_aligned = rotation * sigma * rotation.transpose(); double det = sigma_aligned.determinant(); // if( std::isfinite(det) && det>0) // pt.probability = 1 / sqrt(2 * M_PI * det); // else // pt.probability = std::numeric_limits<float>::min(); if( std::isfinite(det) && det>0) pt.weight = det; else pt.weight = std::numeric_limits<float>::max(); } point_count += kept_new_pts; } big_cloud_info_.resize(point_count); }
void v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized) { models_.clear(); if(input_->isOrganized() && !force_unorganized) { pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); if(!normals_set_) { pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); ne.setNormalSmoothingSize (20.0f); ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE); ne.setInputCloud (input_); ne.compute (*normal_cloud); } else { normal_cloud = normal_cloud_; } pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (min_plane_inliers_); mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees mps.setDistanceThreshold (0.01); // 1cm mps.setMaximumCurvature(0.002); mps.setInputNormals (normal_cloud); mps.setInputCloud (input_); std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; std::vector<pcl::ModelCoefficients> model_coefficients; std::vector<pcl::PointIndices> inlier_indices; pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>); std::vector<pcl::PointIndices> label_indices; std::vector<pcl::PointIndices> boundary_indices; typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp ( new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); ref_comp->setDistanceThreshold (0.01f, false); ref_comp->setAngularThreshold (0.017453 * 2.f); mps.setRefinementComparator (ref_comp); mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); //mps.segment (model_coefficients, inlier_indices); //std::cout << model_coefficients.size() << std::endl; if(merge_planes_) { //sort planes by size //check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane; GraphPlane mergeable_planes (model_coefficients.size ()); for(size_t i=0; i < model_coefficients.size(); i++) { Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1], model_coefficients[i].values[2]); plane_i.normalize(); for(size_t j=(i+1); j < model_coefficients.size(); j++) { Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1], model_coefficients[j].values[2]); plane_j.normalize(); //std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl; if(plane_i.dot(plane_j) > 0.95) { if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015) { boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes); } } } } boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes)); int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0])); std::vector<int> cc_sizes; std::vector< std::vector<int> > cc_to_model_coeff; cc_sizes.resize (n_cc, 0); cc_to_model_coeff.resize(n_cc); for (size_t i = 0; i < model_coefficients.size (); i++) { cc_sizes[components[i]]++; cc_to_model_coeff[components[i]].push_back(i); } std::vector<pcl::ModelCoefficients> new_model_coefficients; std::vector<pcl::PointIndices> new_inlier_indices; for(size_t i=0; i < cc_sizes.size(); i++) { if(cc_sizes[i] < 2) { new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]); new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]); continue; } //std::cout << "going to merge CC:" << cc_sizes[i] << std::endl; pcl::ModelCoefficients model_coeff; model_coeff.values.resize(4); for(size_t k=0; k < 4; k++) model_coeff.values[k] = 0.f; pcl::PointIndices merged_indices; for(size_t j=0; j < cc_to_model_coeff[i].size(); j++) { for(size_t k=0; k < 4; k++) model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k]; merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(), inlier_indices[cc_to_model_coeff[i][j]].indices.end()); } for(size_t k=0; k < 4; k++) model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size()); new_model_coefficients.push_back(model_coeff); new_inlier_indices.push_back(merged_indices); } model_coefficients = new_model_coefficients; inlier_indices = new_inlier_indices; } for(size_t i=0; i < model_coefficients.size(); i++) { PlaneModel<PointT> pm; pm.coefficients_ = model_coefficients[i]; pm.cloud_ = input_; pm.inliers_ = inlier_indices[i]; //recompute coefficients based on distance to camera and normal? Eigen::Vector4f centroid; pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid); Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]); Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3); float sum_w = 0.f; for(size_t k=0; k < inlier_indices[i].indices.size(); k++) { const int &idx = inlier_indices[i].indices[k]; float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm(); float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f); //w_k = 1.f; M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c); sum_w += w_k; } Eigen::Matrix3f scatter; scatter.setZero (); scatter = M_w.transpose() * M_w; Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV); //std::cout << svd.matrixV() << std::endl; Eigen::Vector3f n = svd.matrixV().col(2); //flip normal if required if(n.dot(c*-1) < 0) n = n * -1.f; float d = n.dot(c) * -1.f; //std::cout << "normal:" << n << std::endl; //std::cout << "d:" << d << std::endl; pm.coefficients_.values[0] = n[0]; pm.coefficients_.values[1] = n[1]; pm.coefficients_.values[2] = n[2]; pm.coefficients_.values[3] = d; pcl::PointIndices clean_inlier_indices; float dist_threshold_ = 0.01f; for(size_t k=0; k < inlier_indices[i].indices.size(); k++) { const int &idx = inlier_indices[i].indices[k]; Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap(); float val = n.dot(p) + d; if(std::abs(val) <= dist_threshold_) clean_inlier_indices.indices.push_back( idx ); } pm.inliers_ = clean_inlier_indices; models_.push_back(pm); } } else { // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<PointT> vg; PointTCloudPtr cloud_filtered (new PointTCloud); vg.setInputCloud (input_); float leaf_size_ = 0.005f; float dist_threshold_ = 0.01f; vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_); vg.filter (*cloud_filtered); std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true ); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<PointT> seg; pcl::ModelCoefficients coefficients; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (dist_threshold_); PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered)); while (1) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered_leftover); pcl::PointIndices inliers_in_leftover; seg.segment (inliers_in_leftover, coefficients); std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl; if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud. break; // make indices correspond to complete downsampled cloud pcl::PointIndices indices_in_original_cloud; size_t current_index_in_leftover = 0; size_t px=0; indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size()); for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) { // assumes indices are sorted in ascending order bool found = false; do { if( pixel_has_not_been_labelled[px] ) { if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) { indices_in_original_cloud.indices[ inl_id ] = px; found = true; } current_index_in_leftover++; } px++; } while( !found ); } for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++) pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false; //save coefficients PlaneModel<PointT> pm; pm.coefficients_ = coefficients; pm.cloud_ = cloud_filtered; pm.inliers_ = indices_in_original_cloud; models_.push_back(pm); pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover); } std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl; } }
void PwnTracker::processFrame(const DepthImage& depthImage_, const Eigen::Isometry3f& sensorOffset_, const Eigen::Matrix3f& cameraMatrix_, const Eigen::Isometry3f& initialGuess){ int r,c; Eigen::Matrix3f scaledCameraMatrix = cameraMatrix_; _currentCloudOffset = sensorOffset_; _currentCameraMatrix = cameraMatrix_; _currentDepthImage = depthImage_; _currentCloud = makeCloud(r,c,scaledCameraMatrix,_currentCloudOffset,_currentDepthImage); bool newFrame = false; bool newRelation = false; PwnTrackerFrame* currentTrackerFrame=0; Eigen::Isometry3d relationMean; if (_previousCloud){ _aligner->setCurrentSensorOffset(_currentCloudOffset); _aligner->setCurrentFrame(_currentCloud); _aligner->setReferenceSensorOffset(_previousCloudOffset); _aligner->setReferenceFrame(_previousCloud); _aligner->correspondenceFinder()->setImageSize(r,c); PinholePointProjector* alprojector=dynamic_cast<PinholePointProjector*>(_aligner->projector()); alprojector->setCameraMatrix(scaledCameraMatrix); alprojector->setImageSize(r,c); Eigen::Isometry3f guess=_previousCloudTransform.inverse()*_globalT*initialGuess; _aligner->setInitialGuess(guess); double t0, t1; t0 = g2o::get_time(); _aligner->align(); t1 = g2o::get_time(); std::cout << "Time: " << t1 - t0 << " seconds " << std::endl; cerr << "inliers: " << _aligner->inliers() << endl; // cerr << "chi2: " << _aligner->error() << endl; // cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl; // cerr << "initialGuess: " << t2v(guess).transpose() << endl; // cerr << "transform : " << t2v(_aligner->T()).transpose() << endl; if (_aligner->inliers()>0){ _globalT = _previousCloudTransform*_aligner->T(); //cerr << "TRANSFORM FOUND" << endl; } else { //cerr << "FAILURE" << endl; _globalT = _globalT*guess; } convertScalar(relationMean, _aligner->T()); if (! (_counter%50) ) { Eigen::Matrix3f R = _globalT.linear(); Eigen::Matrix3f E = R.transpose() * R; E.diagonal().array() -= 1; _globalT.linear() -= 0.5 * R * E; } _globalT.matrix().row(3) << 0,0,0,1; newAlignmentCallbacks(_globalT, _aligner->T(), _aligner->inliers(), _aligner->error()); int maxInliers = r*c; float inliersFraction = (float) _aligner->inliers()/(float) maxInliers; cerr << "inliers/maxinliers/fraction: " << _aligner->inliers() << "/" << maxInliers << "/" << inliersFraction << endl; if (inliersFraction<_newFrameInliersFraction){ newFrame = true; newRelation = true; // char filename[1024]; // sprintf (filename, "frame-%05d.pwn", _numKeyframes); // frame->save(filename,1,true,_globalT); _numKeyframes ++; if (!_cache) delete _previousCloud; else{ _previousCloudHandle.release(); } //_cache->unlock(_previousTrackerFrame); // _aligner->setReferenceSensorOffset(_currentCloudOffset); // _aligner->setReferenceFrame(_currentCloud); _previousCloud = _currentCloud; _previousCloudTransform = _globalT; // cerr << "new frame added (" << _numKeyframes << ")" << endl; // cerr << "inliers: " << _aligner->inliers() << endl; // cerr << "maxInliers: " << maxInliers << endl; // cerr << "chi2: " << _aligner->error() << endl; // cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl; // cerr << "initialGuess: " << t2v(guess).transpose() << endl; // cerr << "transform : " << t2v(_aligner->T()).transpose() << endl; // cerr << "globalTransform : " << t2v(_globalT).transpose() << endl; } else { // previous frame but offset is small delete _currentCloud; _currentCloud = 0; } } else { // first frame //ser.writeObject(*manager); newFrame = true; // _aligner->setReferenceSensorOffset(_currentCloudOffset); // _aligner->setReferenceFrame(_currentCloud); _previousCloud = _currentCloud; _previousCloudTransform = _globalT; _previousCloudOffset = _currentCloudOffset; _numKeyframes ++; /*Eigen::Isometry3f t = _globalT; geometry_msgs::Point p; p.x = t.translation().x(); p.y = t.translation().y(); p.z = t.translation().z(); m_odometry.points.push_back(p); */ } _counter++; if (newFrame) { //cerr << "maing new frame, previous: " << _previousTrackerFrame << endl; currentTrackerFrame = new PwnTrackerFrame(_manager); //currentTrackerFrame->cloud.set(cloud); currentTrackerFrame->cloud=_currentCloud; currentTrackerFrame->sensorOffset = _currentCloudOffset; boss_map::ImageBLOB* depthBLOB=new boss_map::ImageBLOB(); DepthImage_convert_32FC1_to_16UC1(depthBLOB->cvImage(),_currentDepthImage); //depthImage.toCvMat(depthBLOB->cvImage()); depthBLOB->adjustFormat(); currentTrackerFrame->depthImage.set(depthBLOB); boss_map::ImageBLOB* normalThumbnailBLOB=new boss_map::ImageBLOB(); boss_map::ImageBLOB* depthThumbnailBLOB=new boss_map::ImageBLOB(); makeThumbnails(depthThumbnailBLOB->cvImage(), normalThumbnailBLOB->cvImage(), _currentCloud, _currentDepthImage.rows, _currentDepthImage.cols, _currentCloudOffset, _currentCameraMatrix, 0.1); normalThumbnailBLOB->adjustFormat(); depthThumbnailBLOB->adjustFormat(); currentTrackerFrame->depthThumbnail.set(depthThumbnailBLOB); currentTrackerFrame->normalThumbnail.set(normalThumbnailBLOB); currentTrackerFrame->cameraMatrix = _currentCameraMatrix; currentTrackerFrame->imageRows = _currentDepthImage.rows; currentTrackerFrame->imageCols = _currentDepthImage.cols; Eigen::Isometry3d _iso; convertScalar(_iso, _globalT); currentTrackerFrame->setTransform(_iso); convertScalar(currentTrackerFrame->sensorOffset, _currentCloudOffset); currentTrackerFrame->setSeq(_seq++); _manager->addNode(currentTrackerFrame); //cerr << "AAAA" << endl; if (_cache) _currentCloudHandle=_cache->get(currentTrackerFrame); //cerr << "BBBB" << endl; //_cache->lock(currentTrackerFrame); newFrameCallbacks(currentTrackerFrame); currentTrackerFrame->depthThumbnail.set(0); currentTrackerFrame->normalThumbnail.set(0); currentTrackerFrame->depthImage.set(0); } if (newRelation) { PwnTrackerRelation* rel = new PwnTrackerRelation(_manager); rel->setTransform(relationMean); Matrix6d omega; convertScalar(omega, _aligner->omega()); omega.setIdentity(); omega *= 100; rel->setInformationMatrix(omega); rel->setTo(currentTrackerFrame); rel->setFrom(_previousTrackerFrame); //cerr << "saved relation" << _previousTrackerFrame << " " << currentTrackerFrame << endl; _manager->addRelation(rel); newRelationCallbacks(rel); //ser.writeObject(*rel); } if (currentTrackerFrame) { _previousTrackerFrame = currentTrackerFrame; } }
template <typename PointInT, typename PointOutT> void pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const { const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ()); std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles); std::vector <float> triangle_area (number_of_triangles); std::vector <float> distance_weight (number_of_triangles); float total_area = 0.0f; const float coeff = 1.0f / 12.0f; const float coeff_1_div_3 = 1.0f / 3.0f; Eigen::Vector3f feature_point (point.x, point.y, point.z); std::set <unsigned int>::const_iterator it; unsigned int i_triangle = 0; for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++) { Eigen::Vector3f pt[3]; for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) { const unsigned int index = triangles_[*it].vertices[i_vertex]; pt[i_vertex] (0) = surface_->points[index].x; pt[i_vertex] (1) = surface_->points[index].y; pt[i_vertex] (2) = surface_->points[index].z; } const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm (); triangle_area[i_triangle] = curr_area; total_area += curr_area; distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f); Eigen::Matrix3f curr_scatter_matrix; curr_scatter_matrix.setZero (); for (unsigned int i_pt = 0; i_pt < 3; i_pt++) { Eigen::Vector3f vec = pt[i_pt] - feature_point; curr_scatter_matrix += vec * (vec.transpose ()); for (unsigned int j_pt = 0; j_pt < 3; j_pt++) curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ()); } scatter_matrices[i_triangle] = coeff * curr_scatter_matrix; } if (std::abs (total_area) < std::numeric_limits <float>::epsilon ()) total_area = 1.0f / total_area; else total_area = 1.0f; Eigen::Matrix3f overall_scatter_matrix; overall_scatter_matrix.setZero (); std::vector<float> total_weight (number_of_triangles); const float denominator = 1.0f / 6.0f; for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++) { float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area; overall_scatter_matrix += factor * scatter_matrices[i_triangle]; total_weight[i_triangle] = factor * denominator; } Eigen::Vector3f v1, v2, v3; computeEigenVectors (overall_scatter_matrix, v1, v2, v3); float h1 = 0.0f; float h3 = 0.0f; for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++) { Eigen::Vector3f pt[3]; for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) { const unsigned int index = triangles_[*it].vertices[i_vertex]; pt[i_vertex] (0) = surface_->points[index].x; pt[i_vertex] (1) = surface_->points[index].y; pt[i_vertex] (2) = surface_->points[index].z; } float factor1 = 0.0f; float factor3 = 0.0f; for (unsigned int i_pt = 0; i_pt < 3; i_pt++) { Eigen::Vector3f vec = pt[i_pt] - feature_point; factor1 += vec.dot (v1); factor3 += vec.dot (v3); } h1 += total_weight[i_triangle] * factor1; h3 += total_weight[i_triangle] * factor3; } if (h1 < 0.0f) v1 = -v1; if (h3 < 0.0f) v3 = -v3; v2 = v3.cross (v1); lrf_matrix.row (0) = v1; lrf_matrix.row (1) = v2; lrf_matrix.row (2) = v3; }
void pcl::getCameraMatrixFromProjectionMatrix ( const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, Eigen::Matrix3f& camera_matrix) { Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> (); Eigen::Matrix3f KR_KRT = KR * KR.transpose (); Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8); memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9); camera_matrix.coeffRef (8) = 1.0; if (camera_matrix.Flags & Eigen::RowMajorBit) { camera_matrix.coeffRef (2) = cam.coeff (2); camera_matrix.coeffRef (5) = cam.coeff (5); camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5))); camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4); camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2))); } else { camera_matrix.coeffRef (6) = cam.coeff (2); camera_matrix.coeffRef (7) = cam.coeff (5); camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5))); camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4); camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2))); } }
bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg, typename pcl::PointCloud<T>::Ptr hand_cloud, //typename pcl::PointCloud<T>::Ptr finger_cloud, const std::string& frame_id, tf::Vector3& hand_position, tf::Vector3& arm_position, tf::Vector3& arm_direction) { typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>); pcl::fromROSMsg(cloud_msg, *cloud); if((cloud->points.size() < g_config.min_cluster_size) || (cloud->points.size() > g_config.max_cluster_size)) return false; pcl::PCA<T> pca; pca.setInputCloud(cloud); Eigen::Vector4f mean = pca.getMean(); if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false; if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false; if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false; Eigen::Vector3f eigen_value = pca.getEigenValues(); double ratio = eigen_value.coeff(0) / eigen_value.coeff(1); if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false; T search_point; Eigen::Matrix3f ev = pca.getEigenVectors(); Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0)); main_axis.normalize(); arm_direction.setX(main_axis.coeff(0)); arm_direction.setY(main_axis.coeff(1)); arm_direction.setZ(main_axis.coeff(2)); arm_position.setX(mean.coeff(0)); arm_position.setY(mean.coeff(1)); arm_position.setZ(mean.coeff(2)); main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2)); search_point.x = main_axis.coeff(0); search_point.y = main_axis.coeff(1); search_point.z = main_axis.coeff(2); //find hand pcl::KdTreeFLANN<T> kdtree; kdtree.setInputCloud(cloud); //find the closet point from the serach_point std::vector<int> point_indeices(1); std::vector<float> point_distances(1); if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 ) { //update search point search_point = cloud->points[point_indeices[0]]; //show seach point if(g_marker_array_pub.getNumSubscribers() != 0) { pushSimpleMarker(search_point.x, search_point.y, search_point.z, 1.0, 0, 0, 0.02, g_marker_id, g_marker_array, frame_id); } //hand point_indeices.clear(); point_distances.clear(); kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances); for (size_t i = 0; i < point_indeices.size (); ++i) { hand_cloud->points.push_back(cloud->points[point_indeices[i]]); hand_cloud->points.back().r = 255; hand_cloud->points.back().g = 0; hand_cloud->points.back().b = 0; } Eigen::Vector4f centroid; pcl::compute3DCentroid(*hand_cloud, centroid); if(g_marker_array_pub.getNumSubscribers() != 0) { pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2), 0.0, 1.0, 0, 0.02, g_marker_id, g_marker_array, frame_id); } hand_position.setX(centroid.coeff(0)); hand_position.setY(centroid.coeff(1)); hand_position.setZ(centroid.coeff(2)); #if 0 //fingers search_point.x = centroid.coeff(0); search_point.y = centroid.coeff(1); search_point.z = centroid.coeff(2); std::vector<int> point_indeices_inner; std::vector<float> point_distances_inner; kdtree.radiusSearch(search_point, 0.07, point_indeices_inner, point_distances_inner); std::vector<int> point_indeices_outter; std::vector<float> point_distances_outter; kdtree.radiusSearch(search_point, 0.1, point_indeices_outter, point_distances_outter); //ROS_INFO("before %d %d", point_indeices_inner.size(), point_indeices_outter.size()); std::vector<int>::iterator it; for(size_t i = 0; i < point_indeices_inner.size(); i++) { it = std::find(point_indeices_outter.begin(), point_indeices_outter.end(), point_indeices_inner[i]); if(it != point_indeices_outter.end()) { point_indeices_outter.erase(it); } } //ROS_INFO("after %d %d", point_indeices_inner.size(), point_indeices_outter.size()); //ROS_DEBUG_THROTTLE(1.0, "found %lu", point_indeices.size ()); for (size_t i = 0; i < point_indeices_outter.size (); ++i) { finger_cloud->points.push_back(cloud->points[point_indeices_outter[i]]); finger_cloud->points.back().r = 255; finger_cloud->points.back().g = 0; finger_cloud->points.back().b = 0; } #endif } else { return false; } if(g_marker_array_pub.getNumSubscribers() != 0) { pushEigenMarker<T>(pca, g_marker_id, g_marker_array, 0.1, frame_id); } return true; }
void ViewerState::load(const std::string& filename){ clear(); listWidget->clear(); graph->clear(); graph->load(filename.c_str()); vector<int> vertexIds(graph->vertices().size()); int k=0; for (OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) { vertexIds[k++] = (it->first); } sort(vertexIds.begin(), vertexIds.end()); listAssociations.clear(); size_t maxCount = 20000; for(size_t i = 0; i < vertexIds.size() && i< maxCount; ++i) { OptimizableGraph::Vertex* _v = graph->vertex(vertexIds[i]); g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(_v); if (! v) continue; OptimizableGraph::Data* d = v->userData(); while(d) { RGBDData* rgbdData = dynamic_cast<RGBDData*>(d); if (!rgbdData){ d=d->next(); continue; } // retrieve from the rgb data the index of the parameter int paramIndex = rgbdData->paramIndex(); // retrieve from the graph the parameter given the index g2o::Parameter* _cameraParam = graph->parameter(paramIndex); // attempt a cast to a parameter camera ParameterCamera* cameraParam = dynamic_cast<ParameterCamera*>(_cameraParam); if (! cameraParam){ cerr << "shall thou be damned forever" << endl; return; } // yayyy we got the parameter Eigen::Matrix3f cameraMatrix; Eigen::Isometry3f sensorOffset; cameraMatrix.setZero(); int cmax = 4; int rmax = 3; for (int c=0; c<cmax; c++){ for (int r=0; r<rmax; r++){ sensorOffset.matrix()(r,c)= cameraParam->offset()(r, c); if (c<3) cameraMatrix(r,c) = cameraParam->Kcam()(r, c); } } char buf[1024]; sprintf(buf,"%d",v->id()); QString listItem(buf); listAssociations.push_back(rgbdData); listWidget->addItem(listItem); d=d->next(); } } }