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();
}
}
}
