IGL_INLINE bool igl::embree::unproject_onto_mesh(
const Eigen::Vector2f& pos,
const Eigen::MatrixXi& F,
const Eigen::Matrix4f& model,
const Eigen::Matrix4f& proj,
const Eigen::Vector4f& viewport,
const EmbreeIntersector & ei,
int& fid,
int& vid)
{
Eigen::Vector3f bc;
bool hit = unproject_onto_mesh(pos,F,model,proj,viewport,ei,fid,bc);
int i;
if (hit)
{
bc.maxCoeff(&i);
vid = F(fid,i);
}
return hit;
}
bool LcmTranslator::
toLcm(const PointCloudView& iView, drc::map_cloud_t& oMessage,
const float iQuantMax, const bool iCompress) {
oMessage.view_id = iView.getId();
// find extrema of cloud and transform points
maps::PointCloud::PointType maxPoint, minPoint;
pcl::getMinMax3D(*(iView.getPointCloud()), minPoint, maxPoint);
Eigen::Vector3f offset = minPoint.getVector3fMap();
Eigen::Vector3f scale = maxPoint.getVector3fMap() - offset;
int bits = 8;
if (iQuantMax == 0) {
bits = 32;
scale << 1,1,1;
}
else {
if (iQuantMax > 0) {
bits = ceil(std::log2(scale.maxCoeff()/iQuantMax));
bits = std::min(bits, 16);
bits = std::max(bits, 0);
}
scale /= ((1 << bits) - 1);
}
Eigen::Affine3f normalizerInv = Eigen::Affine3f::Identity();
for (int i = 0; i < 3; ++i) {
normalizerInv(i,i) = scale[i];
normalizerInv(i,3) = offset[i];
}
Eigen::Affine3f normalizer = normalizerInv.inverse();
maps::PointCloud cloud;
pcl::transformPointCloud(*(iView.getPointCloud()), cloud, normalizer);
// store to blob
int totalSize = cloud.points.size()*3;
std::vector<uint8_t> data(totalSize*sizeof(float));
float* ptr = (float*)(&data[0]);
for (int i = 0; i < cloud.size(); ++i) {
maps::PointCloud::PointType pt = cloud.points[i];
ptr[i*3+0] = pt.x + 0.5f; // add 0.5 for better rounding
ptr[i*3+1] = pt.y + 0.5f;
ptr[i*3+2] = pt.z + 0.5f;
}
DataBlob::Spec spec;
spec.mDimensions.push_back(3);
spec.mDimensions.push_back(cloud.size());
spec.mStrideBytes.push_back(sizeof(float));
spec.mStrideBytes.push_back(3*sizeof(float));
spec.mCompressionType = DataBlob::CompressionTypeNone;
spec.mDataType = DataBlob::DataTypeFloat32;
DataBlob blob;
blob.setData(data, spec);
// compress and convert
DataBlob::CompressionType compressionType =
iCompress ? DataBlob::CompressionTypeZlib : DataBlob::CompressionTypeNone;
DataBlob::DataType dataType;
if (bits <= 8) dataType = DataBlob::DataTypeUint8;
else if (bits <= 16) dataType = DataBlob::DataTypeUint16;
else dataType = DataBlob::DataTypeFloat32;
if (!blob.convertTo(compressionType, dataType)) {
std::cout << "LcmTranslator: cannot compress data" << std::endl;
return false;
}
// pack blob into message
if (!toLcm(blob, oMessage.blob)) return false;
// transform
Eigen::Projective3f fromRef = normalizer*iView.getTransform();
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
oMessage.transform[i][j] = fromRef(i,j);
}
}
// done
// NOTE: map_id not set
return true;
}