void ndtTransformAndAdd(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& A,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& B)
{
pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB> ndt;
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon (0.01);
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize (0.1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution (1.0);
// Setting max number of registration iterations.
ndt.setMaximumIterations (35);
// Setting point cloud to be aligned.
ndt.setInputSource (B);
// Setting point cloud to be aligned to.
ndt.setInputTarget (A);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f estimate;
estimate.setIdentity();
ndt.align (*output_cloud, estimate.matrix());
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
// Transforming unfiltered, input cloud using found transform.
pcl::transformPointCloud (*B, *output_cloud, ndt.getFinalTransformation ());
*A=*A+*output_cloud;
}
bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
Eigen::Affine3f& transf)
{
// uz: versor pointing toward the destination
Eigen::Vector3f uz = target - rayo;
if (std::abs(uz.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
return false;
}
uz.normalize();
//std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
// ux: versor pointing toward the ground
Eigen::Vector3f ux = down - down.dot(uz) * uz;
if (std::abs(ux.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
return false;
}
ux.normalize();
//std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
Eigen::Vector3f uy = uz.cross(ux);
//std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
Eigen::Matrix3f rot;
rot << ux.x(), uy.x(), uz.x(),
ux.y(), uy.y(), uz.y(),
ux.z(), uy.z(), uz.z();
transf.setIdentity();
transf.translate(rayo);
transf.rotate(rot);
//std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
return true;
}
void CData4Viewer::drawCameraView(qglviewer::Camera* pCamera_)
{
_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);
glMatrixMode(GL_MODELVIEW);
Eigen::Affine3f prj_w_t_c; _pTracker->getCurrentProjectionMatrix(&prj_w_t_c);
Eigen::Affine3f init; init.setIdentity(); init(1, 1) = -1.f; init(2, 2) = -1.f;// rotate the default opengl camera orientation to make it facing positive z
glLoadMatrixf(init.data());
glMultMatrixf(prj_w_t_c.data());
//if(_bShowCamera) {
// _pTracker->displayGlobalRelocalization();
//}
//if(_bShowMarkers) {
// _pTracker->displayAllGlobalFeatures(_nVoxelLevel,_bRenderSphere);
//}
_pVirtualCameraView->assignRTfromGL();
_pCubicGrids->rayCast(&*_pVirtualCameraView,true,_bCapture); //get virtual frame
bool bLightingStatus = _pGL->_bEnableLighting;
_pGL->_bEnableLighting = true;
_pVirtualCameraView->gpuRender3DPts(_pGL.get(),_pGL->_usLevel);
_pGL->_bEnableLighting = bLightingStatus;
//PRINTSTR("drawCameraView");
return;
}
PointCloudViewer::PointCloudViewer(QWidget* parent, Qt::WindowFlags f): QVTKWidget(parent, f)
{
mImpl = new PointCloudViewer::Impl;
mImpl->Vis.addPointCloud(common::KinectPointCloud::Ptr(new common::KinectPointCloud));
Eigen::Affine3f trans;
trans.setIdentity();
trans.rotate(Eigen::AngleAxisf(3.14159265, Eigen::Vector3f(0, 0, 1)));
mImpl->Vis.addCoordinateSystem(1.0, trans);
mImpl->Vis.setBackgroundColor(0, 0, 0);
SetRenderWindow(mImpl->Vis.getRenderWindow().GetPointer());
}
void CData4Viewer::drawRGBView()
{
_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);
glMatrixMode ( GL_MODELVIEW );
Eigen::Affine3f tmp; tmp.setIdentity();
Matrix4f mv = btl::utility::setModelViewGLfromPrj(tmp); //mv transform X_m to X_w i.e. model coordinate to world coordinate
glLoadMatrixf( mv.data() );
_pKinect->_pRGBCamera->renderCameraInLocal(*_pKinect->_pCurrFrame->_acvgmShrPtrPyrRGBs[_pGL->_usLevel], _pGL.get(),false, NULL, 0.2f, true ); //render in model coordinate
//PRINTSTR("drawRGBView");
return;
}
int
main (int argc, char** argv)
{
// Get input object and scene
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
return (1);
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if(argc<3)
{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
pcl::console::print_error ("Error loading first file!\n");
*cloud_out = *cloud_in;
//transform cloud
Eigen::Affine3f transformation;
transformation.setIdentity();
transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
float roll, pitch, yaw;
roll = 0.02; pitch = 1.2; yaw = 0;
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
transformation.rotate(q);
pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
}else{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
{
pcl::console::print_error ("Error loading files!\n");
return (1);
}
}
// Fill in the CloudIn data
// cloud_in->width = 100;
// cloud_in->height = 1;
// cloud_in->is_dense = false;
// cloud_in->points.resize (cloud_in->width * cloud_in->height);
// for (size_t i = 0; i < cloud_in->points.size (); ++i)
// {
// cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
// }
std::cout << "size:" << cloud_out->points.size() << std::endl;
{
pcl::ScopeTime("icp proces");
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZRGBA> Final;
icp.setMaximumIterations(1000000);
icp.setRANSACOutlierRejectionThreshold(0.01);
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
//translation, rotation
Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
std::cout << "rotation: " << euler.transpose() << std::endl;
std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
}
return (0);
}
void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
JSK_ROS_DEBUG("DepthImageCreator::publish_points");
if (!pcloud2) return;
bool proc_cloud = true, proc_image = true, proc_disp = true;
if ( pub_cloud_.getNumSubscribers()==0 ) {
proc_cloud = false;
}
if ( pub_image_.getNumSubscribers()==0 ) {
proc_image = false;
}
if ( pub_disp_image_.getNumSubscribers()==0 ) {
proc_disp = false;
}
if( !proc_cloud && !proc_image && !proc_disp) return;
int width = info->width;
int height = info->height;
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
Eigen::Affine3f sensorPose;
{
tf::StampedTransform transform;
if(use_fixed_transform) {
transform = fixed_transform;
} else {
try {
tf_listener_->waitForTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp,
ros::Duration(0.001));
tf_listener_->lookupTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp, transform);
}
catch ( std::runtime_error e ) {
JSK_ROS_ERROR("%s",e.what());
return;
}
}
tf::Vector3 p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
sensorPose = sensorPose * rot;
if (tx != 0.0) {
Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
sensorPose = sensorPose * trans;
}
#if 0 // debug print
JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
}
PointCloud pointCloud;
pcl::RangeImagePlanar rangeImageP;
{
// code here is dirty, some bag is in RangeImagePlanar
PointCloud tpc;
pcl::fromROSMsg(*pcloud2, tpc);
Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
inv = sensorPose.inverse();
pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
pcl::getInverse(sensorPose, inv);
pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif
Eigen::Affine3f dummytrans;
dummytrans.setIdentity();
rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
width/scale_depth, height/scale_depth,
cx/scale_depth, cy/scale_depth,
fx/scale_depth, fy/scale_depth,
dummytrans); //sensorPose);
}
cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
float *tmpf = (float *)mat.ptr();
for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
tmpf[i] = rangeImageP.points[i].z;
}
if(scale_depth != 1.0) {
cv::Mat tmpmat(info->height, info->width, CV_32FC1);
cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
//cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
mat = tmpmat;
}
if (proc_image) {
sensor_msgs::Image pubimg;
pubimg.header = info->header;
pubimg.width = info->width;
pubimg.height = info->height;
pubimg.encoding = "32FC1";
pubimg.step = sizeof(float)*info->width;
pubimg.data.resize(sizeof(float)*info->width*info->height);
// publish image
memcpy(&(pubimg.data[0]), mat.ptr(), sizeof(float)*info->height*info->width);
pub_image_.publish(boost::make_shared<sensor_msgs::Image>(pubimg));
}
if(proc_cloud || proc_disp) {
// publish point cloud
pcl::RangeImagePlanar rangeImagePP;
rangeImagePP.setDepthImage ((float *)mat.ptr(),
width, height,
cx, cy, fx, fy);
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
rangeImagePP.header = pcl_conversions::toPCL(info->header);
#else
rangeImagePP.header = info->header;
#endif
if(proc_cloud) {
pub_cloud_.publish(boost::make_shared<pcl::PointCloud<pcl::PointWithRange > >
( (pcl::PointCloud<pcl::PointWithRange>)rangeImagePP) );
}
if(proc_disp) {
stereo_msgs::DisparityImage disp;
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
#else
disp.header = rangeImagePP.header;
#endif
disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
disp.image.height = rangeImagePP.height;
disp.image.width = rangeImagePP.width;
disp.image.step = disp.image.width * sizeof(float);
disp.f = fx; disp.T = 0.075;
disp.min_disparity = 0;
disp.max_disparity = disp.T * disp.f / 0.3;
disp.delta_d = 0.125;
disp.image.data.resize (disp.image.height * disp.image.step);
float *data = reinterpret_cast<float*> (&disp.image.data[0]);
float normalization_factor = disp.f * disp.T;
for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
data[y*disp.image.width+x] = normalization_factor / p.z;
}
}
pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
}
}
}
NORI_NAMESPACE_BEGIN
NoriObject *loadFromXML(const std::string &filename) {
/* Load the XML file using 'pugi' (a tiny self-contained XML parser implemented in C++) */
pugi::xml_document doc;
pugi::xml_parse_result result = doc.load_file(filename.c_str());
/* Helper function: map a position offset in bytes to a more readable row/column value */
auto offset = [&](ptrdiff_t pos) -> std::string {
std::fstream is(filename);
char buffer[1024];
int line = 0, linestart = 0, offset = 0;
while (is.good()) {
is.read(buffer, sizeof(buffer));
for (int i = 0; i < is.gcount(); ++i) {
if (buffer[i] == '\n') {
if (offset + i >= pos)
return tfm::format("row %i, col %i", line + 1, pos - linestart);
++line;
linestart = offset + i;
}
}
offset += (int) is.gcount();
}
return "byte offset " + std::to_string(pos);
};
if (!result) /* There was a parser / file IO error */
throw NoriException("Error while parsing \"%s\": %s (at %s)", filename, result.description(), offset(result.offset));
/* Set of supported XML tags */
enum ETag {
/* Object classes */
EScene = NoriObject::EScene,
EMesh = NoriObject::EMesh,
EBSDF = NoriObject::EBSDF,
ETEXTURE = NoriObject::ETEXTURE,
EPERLIN = NoriObject::EPERLIN,
EMIXTEXTURE = NoriObject::EMIXTEXTURE,
EMIXBUMPMAP = NoriObject::EMIXBUMPMAP,
EBUMPMAP = NoriObject::EBUMPMAP,
EPhaseFunction = NoriObject::EPhaseFunction,
EEmitter = NoriObject::EEmitter,
EMedium = NoriObject::EMedium,
EVolume = NoriObject::EVolume,
ECamera = NoriObject::ECamera,
EIntegrator = NoriObject::EIntegrator,
ESampler = NoriObject::ESampler,
ETest = NoriObject::ETest,
EReconstructionFilter = NoriObject::EReconstructionFilter,
/* Properties */
EBoolean = NoriObject::EClassTypeCount,
EInteger,
EFloat,
EString,
EPoint,
EVector,
EColor,
ETransform,
ETranslate,
EMatrix,
ERotate,
EScale,
ELookAt,
EInvalid
};
/* Create a mapping from tag names to tag IDs */
std::map<std::string, ETag> tags;
tags["scene"] = EScene;
tags["mesh"] = EMesh;
tags["bsdf"] = EBSDF;
tags["texture"] = ETEXTURE;
tags["bumpmap"] = EBUMPMAP;
tags["perlin"] = EPERLIN;
tags["mixTexture"] = EMIXTEXTURE;
tags["mixBumpmap"] = EMIXBUMPMAP;
tags["bumpmap"] = EBUMPMAP;
tags["emitter"] = EEmitter;
tags["camera"] = ECamera;
tags["medium"] = EMedium;
tags["volume"] = EVolume;
tags["phase"] = EPhaseFunction;
tags["integrator"] = EIntegrator;
tags["sampler"] = ESampler;
tags["rfilter"] = EReconstructionFilter;
tags["test"] = ETest;
tags["boolean"] = EBoolean;
tags["integer"] = EInteger;
tags["float"] = EFloat;
tags["string"] = EString;
tags["point"] = EPoint;
tags["vector"] = EVector;
tags["color"] = EColor;
tags["transform"] = ETransform;
tags["translate"] = ETranslate;
tags["matrix"] = EMatrix;
tags["rotate"] = ERotate;
tags["scale"] = EScale;
tags["lookat"] = ELookAt;
/* Helper function to check if attributes are fully specified */
auto check_attributes = [&](const pugi::xml_node &node, std::set<std::string> attrs) {
for (auto attr : node.attributes()) {
auto it = attrs.find(attr.name());
if (it == attrs.end())
throw NoriException("Error while parsing \"%s\": unexpected attribute \"%s\" in \"%s\" at %s",
filename, attr.name(), node.name(), offset(node.offset_debug()));
attrs.erase(it);
}
if (!attrs.empty())
throw NoriException("Error while parsing \"%s\": missing attribute \"%s\" in \"%s\" at %s",
filename, *attrs.begin(), node.name(), offset(node.offset_debug()));
};
Eigen::Affine3f transform;
/* Helper function to parse a Nori XML node (recursive) */
std::function<NoriObject *(pugi::xml_node &, PropertyList &, int)> parseTag = [&](
pugi::xml_node &node, PropertyList &list, int parentTag) -> NoriObject * {
/* Skip over comments */
if (node.type() == pugi::node_comment || node.type() == pugi::node_declaration)
return nullptr;
if (node.type() != pugi::node_element)
throw NoriException(
"Error while parsing \"%s\": unexpected content at %s",
filename, offset(node.offset_debug()));
/* Look up the name of the current element */
auto it = tags.find(node.name());
if (it == tags.end())
throw NoriException("Error while parsing \"%s\": unexpected tag \"%s\" at %s",
filename, node.name(), offset(node.offset_debug()));
int tag = it->second;
/* Perform some safety checks to make sure that the XML tree really makes sense */
bool hasParent = parentTag != EInvalid;
bool parentIsObject = hasParent && parentTag < NoriObject::EClassTypeCount;
bool currentIsObject = tag < NoriObject::EClassTypeCount;
bool parentIsTransform = parentTag == ETransform;
bool currentIsTransformOp = tag == ETranslate || tag == ERotate || tag == EScale || tag == ELookAt || tag == EMatrix;
if (!hasParent && !currentIsObject)
throw NoriException("Error while parsing \"%s\": root element \"%s\" must be a Nori object (at %s)",
filename, node.name(), offset(node.offset_debug()));
if (parentIsTransform != currentIsTransformOp)
throw NoriException("Error while parsing \"%s\": transform nodes "
"can only contain transform operations (at %s)",
filename, offset(node.offset_debug()));
if (hasParent && !parentIsObject && !(parentIsTransform && currentIsTransformOp))
throw NoriException("Error while parsing \"%s\": node \"%s\" requires a Nori object as parent (at %s)",
filename, node.name(), offset(node.offset_debug()));
if (tag == EScene)
node.append_attribute("type") = "scene";
else if (tag == ETransform)
transform.setIdentity();
PropertyList propList;
std::vector<NoriObject *> children;
for (pugi::xml_node &ch: node.children()) {
NoriObject *child = parseTag(ch, propList, tag);
if (child)
children.push_back(child);
}
NoriObject *result = nullptr;
try {
if (currentIsObject) {
check_attributes(node, { "type" });
/* This is an object, first instantiate it */
result = NoriObjectFactory::createInstance(
node.attribute("type").value(),
propList
);
if (result->getClassType() != (int) tag) {
throw NoriException(
"Unexpectedly constructed an object "
"of type <%s> (expected type <%s>): %s",
NoriObject::classTypeName(result->getClassType()),
NoriObject::classTypeName((NoriObject::EClassType) tag),
result->toString());
}
/* Add all children */
for (auto ch: children) {
result->addChild(ch);
ch->setParent(result);
}
/* Activate / configure the object */
result->activate();
} else {
/* This is a property */
switch (tag) {
case EString: {
check_attributes(node, { "name", "value" });
list.setString(node.attribute("name").value(), node.attribute("value").value());
}
break;
case EFloat: {
check_attributes(node, { "name", "value" });
list.setFloat(node.attribute("name").value(), toFloat(node.attribute("value").value()));
}
break;
case EInteger: {
check_attributes(node, { "name", "value" });
list.setInteger(node.attribute("name").value(), toInt(node.attribute("value").value()));
}
break;
case EBoolean: {
check_attributes(node, { "name", "value" });
list.setBoolean(node.attribute("name").value(), toBool(node.attribute("value").value()));
}
break;
case EPoint: {
check_attributes(node, { "name", "value" });
list.setPoint(node.attribute("name").value(), Point3f(toVector3f(node.attribute("value").value())));
}
break;
case EVector: {
check_attributes(node, { "name", "value" });
list.setVector(node.attribute("name").value(), Vector3f(toVector3f(node.attribute("value").value())));
}
break;
case EColor: {
check_attributes(node, { "name", "value" });
list.setColor(node.attribute("name").value(), Color3f(toVector3f(node.attribute("value").value()).array()));
}
break;
case ETransform: {
check_attributes(node, { "name" });
list.setTransform(node.attribute("name").value(), transform.matrix());
}
break;
case ETranslate: {
check_attributes(node, { "value" });
Eigen::Vector3f v = toVector3f(node.attribute("value").value());
transform = Eigen::Translation<float, 3>(v.x(), v.y(), v.z()) * transform;
}
break;
case EMatrix: {
check_attributes(node, { "value" });
std::vector<std::string> tokens = tokenize(node.attribute("value").value());
if (tokens.size() != 16)
throw NoriException("Expected 16 values");
Eigen::Matrix4f matrix;
for (int i=0; i<4; ++i)
for (int j=0; j<4; ++j)
matrix(i, j) = toFloat(tokens[i*4+j]);
transform = Eigen::Affine3f(matrix) * transform;
}
break;
case EScale: {
check_attributes(node, { "value" });
Eigen::Vector3f v = toVector3f(node.attribute("value").value());
transform = Eigen::DiagonalMatrix<float, 3>(v) * transform;
}
break;
case ERotate: {
check_attributes(node, { "angle", "axis" });
float angle = degToRad(toFloat(node.attribute("angle").value()));
Eigen::Vector3f axis = toVector3f(node.attribute("axis").value());
transform = Eigen::AngleAxis<float>(angle, axis) * transform;
}
break;
case ELookAt: {
check_attributes(node, { "origin", "target", "up" });
Eigen::Vector3f origin = toVector3f(node.attribute("origin").value());
Eigen::Vector3f target = toVector3f(node.attribute("target").value());
Eigen::Vector3f up = toVector3f(node.attribute("up").value());
Vector3f dir = (target - origin).normalized();
Vector3f left = up.normalized().cross(dir);
Vector3f newUp = dir.cross(left);
Eigen::Matrix4f trafo;
trafo << left, newUp, dir, origin,
0, 0, 0, 1;
transform = Eigen::Affine3f(trafo) * transform;
}
break;
default:
throw NoriException("Unhandled element \"%s\"", node.name());
};
}
} catch (const NoriException &e) {
throw NoriException("Error while parsing \"%s\": %s (at %s)", filename,
e.what(), offset(node.offset_debug()));
}
return result;
};
PropertyList list;
return parseTag(*doc.begin(), list, EInvalid);
}
Eigen::Affine3f OpenNiInterfacePlain::getCloudPose() {
Eigen::Affine3f mid;
mid.setIdentity();
return mid;
}
