void addPath(const Eigen::Vector3f &pt, const Eigen::Vector3f &normal, pcl::PointCloud<pcl::PointNormal> &path) {
pcl::PointNormal start_path;
start_path.x = pt.x(); start_path.y = pt.y(); start_path.z = pt.z();
start_path.normal[0] = normal.x(); start_path.normal[1] = normal.y(); start_path.normal[2] = normal.z();
path.push_back(start_path);
}
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,
EPhaseFunction = NoriObject::EPhaseFunction,
EEmitter = NoriObject::EEmitter,
EMedium = NoriObject::EMedium,
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["emitter"] = EEmitter;
tags["camera"] = ECamera;
tags["medium"] = EMedium;
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);
}
void glsl_program::set_uniform(unsigned loc, const Eigen::Vector3f& value) const
{
if (used_program != this)
throw runtime_error("glsl_program::set_uniform, program not bound!");
glUniform3f(loc, value.x(), value.y(), value.z());
}
_eePose _eePose::applyRPYTo(double roll_z, double pitch_y, double yaw_x) const {
Eigen::Vector3f localUnitX;
{
Eigen::Quaternionf qin(0, 1, 0, 0);
Eigen::Quaternionf qout(0, 1, 0, 0);
Eigen::Quaternionf eeqform(qw, qx, qy, qz);
qout = eeqform * qin * eeqform.conjugate();
localUnitX.x() = qout.x();
localUnitX.y() = qout.y();
localUnitX.z() = qout.z();
}
Eigen::Vector3f localUnitY;
{
Eigen::Quaternionf qin(0, 0, 1, 0);
Eigen::Quaternionf qout(0, 1, 0, 0);
Eigen::Quaternionf eeqform(qw, qx, qy, qz);
qout = eeqform * qin * eeqform.conjugate();
localUnitY.x() = qout.x();
localUnitY.y() = qout.y();
localUnitY.z() = qout.z();
}
Eigen::Vector3f localUnitZ;
{
Eigen::Quaternionf qin(0, 0, 0, 1);
Eigen::Quaternionf qout(0, 1, 0, 0);
Eigen::Quaternionf eeqform(qw, qx, qy, qz);
qout = eeqform * qin * eeqform.conjugate();
localUnitZ.x() = qout.x();
localUnitZ.y() = qout.y();
localUnitZ.z() = qout.z();
}
double sinBuff = 0.0;
double angleRate = 1.0;
Eigen::Quaternionf eeBaseQuat(qw, qx, qy, qz);
sinBuff = sin(angleRate*yaw_x/2.0);
Eigen::Quaternionf eeRotatorX(cos(angleRate*yaw_x/2.0), localUnitX.x()*sinBuff, localUnitX.y()*sinBuff, localUnitX.z()*sinBuff);
sinBuff = sin(angleRate*pitch_y/2.0);
Eigen::Quaternionf eeRotatorY(cos(angleRate*pitch_y/2.0), localUnitY.x()*sinBuff, localUnitY.y()*sinBuff, localUnitY.z()*sinBuff);
sinBuff = sin(angleRate*roll_z/2.0);
Eigen::Quaternionf eeRotatorZ(cos(angleRate*roll_z/2.0), localUnitZ.x()*sinBuff, localUnitZ.y()*sinBuff, localUnitZ.z()*sinBuff);
eeRotatorX.normalize();
eeRotatorY.normalize();
eeRotatorZ.normalize();
eeBaseQuat = eeRotatorX * eeRotatorY * eeRotatorZ * eeBaseQuat;
eeBaseQuat.normalize();
_eePose toreturn;
toreturn.px = px;
toreturn.py = py;
toreturn.pz = pz;
toreturn.qw = eeBaseQuat.w();
toreturn.qx = eeBaseQuat.x();
toreturn.qy = eeBaseQuat.y();
toreturn.qz = eeBaseQuat.z();
return toreturn;
}
/** Computes local depth gradient (depth[m]/distance[m]) */
Eigen::Vector2f computeGradient() const {
return Eigen::Vector2f(normal.x() / normal.z(), normal.y() / normal.z());
}
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft,
const Eigen::Vector3f& localAnkleRight,
const Eigen::Matrix4f& leftFootPoseGroundFrame,
const Eigen::Matrix4f& rightFootPoseGroundFrame,
const Eigen::Vector3f& zmpRefGroundFrame,
Bipedal::SupportPhase phase)
{
Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame);
Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame);
Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse());
Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse());
double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase);
//std::cout << "########## " << alpha << " ###########" << std::endl;
ForceTorque ft;
// kg*m/s^2 = N
ft.leftForce = -(1-alpha) * mass * gravity;
ft.rightForce = -alpha * mass * gravity;
// Note we need force as kg*mm/s^2
ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000);
ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000);
// ZMP not contained in convex polygone
if (std::fabs(alpha) > std::numeric_limits<float>::epsilon()
&& std::fabs(alpha-1) > std::numeric_limits<float>::epsilon())
{
Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque;
Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque;
Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame);
//std::cout << "Tau0World: " << tau0.transpose() << std::endl;
//std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl;
//std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl;
// Note: Our coordinate system is different than in the paper!
// Also this is not the same as the ground frame.
Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft
- localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1);
xAxis /= xAxis.norm();
Eigen::Vector3f zAxis(0, 0, 1);
Eigen::Vector3f yAxis = zAxis.cross(xAxis);
yAxis /= yAxis.norm();
Eigen::Matrix3f centerFrame;
centerFrame.block(0, 0, 3, 1) = xAxis;
centerFrame.block(0, 1, 3, 1) = yAxis;
centerFrame.block(0, 2, 3, 1) = zAxis;
//std::cout << "Center frame:\n" << centerFrame << std::endl;
Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0;
Eigen::Vector3f leftTorqueCenter;
leftTorqueCenter.x() = (1-alpha)*centerTau0.x();
leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0;
leftTorqueCenter.z() = 0;
Eigen::Vector3f rightTorqueCenter;
rightTorqueCenter.x() = alpha*centerTau0.x();
rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y();
rightTorqueCenter.z() = 0;
//std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl;
//std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl;
//std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl;
// tcp <--- ground frame <--- center frame
ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter;
ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter;
}
// Torque depends on timestep, we need a way to do this correctly.
const double torqueFactor = 1;
// convert to Nm
ft.leftTorque *= torqueFactor / 1000.0 / 1000.0;
ft.rightTorque *= torqueFactor / 1000.0 / 1000.0;
//std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl;
//std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl;
return ft;
}
void WorldDownloadManager::initRaycaster(bool has_intrinsics,const kinfu_msgs::KinfuCameraIntrinsics & intr,
bool has_bounding_box_view,const kinfu_msgs::KinfuCloudPoint & bbox_min,const kinfu_msgs::KinfuCloudPoint & bbox_max)
{
const uint rows = has_intrinsics ? intr.size_y : m_kinfu->rows();
const uint cols = has_intrinsics ? intr.size_x : m_kinfu->cols();
float cx,cy,fx,fy;
float min_range = 0.0;
if (has_intrinsics)
{
ROS_INFO("kinfu: custom intrinsics will be used.");
cx = intr.center_x;
cy = intr.center_y;
fx = intr.focal_x;
fy = intr.focal_y;
min_range = intr.min_range;
}
else
m_kinfu->getDepthIntrinsics(fx,fy,cx,cy);
if (!m_raycaster || (uint(m_raycaster->rows) != rows) || (uint(m_raycaster->cols) != cols))
{
ROS_INFO("kinfu: initializing raycaster...");
m_raycaster = RayCaster::Ptr(new RayCaster(rows,cols));
}
m_raycaster->setRaycastStep(m_kinfu->volume().getTsdfTruncDist() * 0.6);
m_raycaster->setMinRange(min_range);
m_raycaster->setIntrinsics(fx,fy,cx,cy);
if (has_bounding_box_view)
{
const Eigen::Vector3f im(bbox_min.x,bbox_min.y,bbox_min.z);
const Eigen::Vector3f iM(bbox_max.x,bbox_max.y,bbox_max.z);
ROS_INFO("kinfu: raycaster will be limited to bounding box: %f %f %f - %f %f %f",im.x(),im.y(),im.z(),iM.x(),iM.y(),iM.z());
const Eigen::Vector3f m = m_reverse_initial_transformation.inverse() * im;
const Eigen::Vector3f M = m_reverse_initial_transformation.inverse() * iM;
const Eigen::Vector3f mmin = m.array().min(M.array());
const Eigen::Vector3f mmax = m.array().max(M.array());
m_raycaster->setBoundingBox(mmin,mmax);
}
else
m_raycaster->clearBoundingBox();
}
int
main (int argc, char** argv)
{
ofstream os("out.g2o");
// /////////////////////////////////////////////////////////////////////////////////////////////////////
int n;
if (argc != 2)
{
std::cerr << "please specify the number of bloody clouds o be registered " << std::endl;
exit (0);
}
n=atoi(argv[1]);
std::cerr << "attempting to read " << n << " bloody clouds" << std::endl;
// /////////////////////////////////////////////////////////////////////////////////////////////////////
// /////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
// sor.setLeafSize (0.01, 0.01, 0.01);
sor.setLeafSize (0.05, 0.05, 0.05);
// sor.setLeafSize (0.05, 0.05, 0.05);
// sor.setLeafSize (0.1, 0.1, 0.1);
// sor.setLeafSize (0.4, 0.4, 0.4);
// sor.setLeafSize (0.5, 0.5, 0.5);
// sor.setInputCloud (inputCloud.makeShared());
// std::cout<<"\n inputCloud.size()="<<inputCloud.size()<<std::endl;
// sor.filter (inputCloudFiltered);
// std::cout<<"\n inputCloudFiltered.size()="<<inputCloudFiltered.size()<<std::endl;
// sor.setInputCloud (targetCloud.makeShared());
// std::cout<<"\n targetCloud.size()="<<targetCloud.size()<<std::endl;
// sor.filter (targetCloudFiltered);
// std::cout<<"\n targetCloudFiltered.size()="<<targetCloudFiltered.size()<<std::endl;
// pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// icp.setMaximumIterations(10000);
// // icp.setMaxCorrespondenceDistance (0.6);
// icp.setMaxCorrespondenceDistance (0.8);
// // icp.setMaxCorrespondenceDistance (1.5);
// // icp.setRANSACOutlierRejectionThreshold (0.1);
// icp.setRANSACOutlierRejectionThreshold (0.6);
// // icp.setRANSACOutlierRejectionThreshold (1.5);
// // icp.setRANSACOutlierRejectionThreshold (5.0);
std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > clouds(n);
std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > filteredClouds(n);
Eigen::Matrix4f tTotal=Eigen::Matrix4f::Identity();
const float min_scale = 0.0005;
const int nr_octaves = 4;
const int nr_scales_per_octave = 5;
const float min_contrast = 1;
//pcl::SIFTKeypoint<pcl::PointXYZRGBA, pcl::PointWithScale> sift;
for (int i=0; i<n; i++){
char filename[20];
char ffilename[20];
sprintf(filename,"base-%03d.pcd", i);
sprintf(ffilename,"filt-%d.pcd", i);
cerr << "filename= " << filename <<endl;
clouds[i]=pcl::PointCloud<pcl::PointXYZRGBA>();
cerr << "ok up to here" << endl;
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (filename, clouds[i]) == -1) //* load the file
{
std::cerr << "Couldn't read the "<< i <<"th pcd file \n" << std::endl;
return (-1);
}
// {
// pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBA> ());;//new API
// pcl::PointCloud<pcl::PointWithScale>::Ptr sifts (new pcl::PointCloud<pcl::PointWithScale>);
// sift.setInputCloud(clouds[i]);
// sift.setSearchMethod (tree);
// sift.setScales(min_scale, nr_octaves, nr_scales_per_octave);
// sift.setMinimumContrast(min_contrast);
// sift.compute (*sifts);
// cerr << "Computed " << sifts->points.size () << " SIFT Keypoints " << endl;
// }
cerr << "sor input" << endl;
sor.setInputCloud (clouds[i].makeShared());
std::cout<<"\n inputCloud.size()="<<clouds[i].size()<<std::endl;
cerr << "sor target" << endl;
filteredClouds[i]=pcl::PointCloud<pcl::PointXYZRGBA>();
cerr << "sor filter" << endl;
sor.filter (filteredClouds[i]);
std::cout<<"\n inputCloudFiltered.size()="<<filteredClouds[i].size()<<std::endl;
pcl::io::savePCDFile(ffilename, filteredClouds[i]);
cerr << "compounding" << endl;
Eigen::Matrix4f transform =Eigen::Matrix4f::Identity();
if (i==0) {
tTotal=Eigen::Matrix4f::Identity();
} else {
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source = filteredClouds[i].makeShared();
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr target = filteredClouds[i-1].makeShared();
Eigen::Matrix4f transform;
pairAlign (source, target, transform, true);
tTotal = tTotal * transform;
}
Eigen::Vector3f t;
Eigen::Quaternionf q;
m2tq(t,q,tTotal);
os << "VERTEX_SE3:QUAT " << i << " "
<< t.x() << " " << t.y() << " " << t.z() << " "
<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
os << "DATA_PCL 0 " << ffilename << " " << i << " 0" << endl;
if (i>0){
Eigen::Vector3f t;
Eigen::Quaternionf q;
m2tq(t,q,transform);
os << "EDGE_SE3:QUAT " << i-1 << " " << i << " "
<< t.x() << " " << t.y() << " " << t.z() << " "
<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";
Eigen::Matrix<float, 6, 6> m= Eigen::Matrix<float, 6, 6>::Identity();
for (int i=0; i<m.rows(); i++)
for (int j=i; j<m.cols(); j++)
os << m(i,j) << " ";
os << endl;
}
}
}
// - afblijven - de update van de vis, deze houd het bewegen van de vis bij en voert een stap uit - afblijven -
void Vis::update(double dt)
{
if (collided >= 0.f)
{
collided -= dt;
if (collided < 0.f)
collided = 0.f;
}
pos += velocity * dt;
Eigen::Vector3f delta = goalPos - pos;
double dist = delta.norm();
if (dist<desired_speed)/// if we're about to pass goal in less than second, change the goal
{
newGoal();
return;
}
/*
//Eigen::Vector3d desiredVelocity=delta*(speed/dist);
if(desiredVelocity.y>velocity.y){/// vertical movement.
velocity.y+=dt*acceleration;
}else{
velocity.y-=dt*acceleration;
}
*/
//horisontale beweging
Eigen::Vector3d fishForward(cos(swimDirAngle), 0, sin(swimDirAngle));
double dfs=dt*forward_acceleration;
speed = StepTowards(speed, desired_speed, dfs);
velocity.x() = fishForward.x() * speed;
velocity.z() = fishForward.z() * speed;
//vertikale beweging
float desired_vertical_speed = delta.y() * speed / dist;// / sqrt(powf(delta.x(), 2.f) * powf(delta.z, 2.f))
float dvs = vertical_acceleration * dt;
velocity.y() = StepTowards(velocity.y(), desired_vertical_speed, dvs);
//double forwardFactor = fishForward.dot(desiredVelocity.normalized());
//velocity+=dt*fishForward*acceleration;// *forwardFactor
/*
double current_speed = velocity.norm();
if(current_speed>speed){/// moving too fast - slow down to exactly speed. TODO: do it smootly for less robotic turns.
velocity*=speed/current_speed;
}
*/
//omdraaien
double goalHeading = atan2(delta.z(), delta.x());
double angleDelta=goalHeading-swimDirAngle;
//maak de kleinst mogelijke draai (-M_PI to +M_PI)
if(angleDelta > M_PI)
{
angleDelta -= 2 * M_PI;
}
if(angleDelta < -M_PI)
{
angleDelta += 2 * M_PI;
}
if (angleDelta > 0.5 * pow(turn_speed, 2.) / turn_acceleration)
{
turn_speed = StepTowards(turn_speed, max_turn_speed, dt * turn_acceleration);
}
else if (angleDelta < -0.5 * pow(turn_speed, 2.) / turn_acceleration)
{
turn_speed = StepTowards(turn_speed, -max_turn_speed, dt * turn_acceleration);
}
else
{
turn_speed = StepTowards(turn_speed, 0., dt * turn_acceleration);
}
swimDirAngle+=dt*turn_speed;
if(swimDirAngle > M_PI)
{
swimDirAngle -= 2 * M_PI;
}
if(swimDirAngle < -M_PI)
{
swimDirAngle += 2 * M_PI;
}
//vibreren
//snelheid van 0 tot oneindig = vibratie van 0 tot wiggle_factor
wiggle_amplitude=wiggle_factor-wiggle_factor/(speed+1.0);
/// magische constante
wiggle_phase = fmod(wiggle_phase + wiggle_freq * dt * speed * wiggle_speed_factor,
2. * M_PI);
//buigen
//buig radius is snelheid / buig snelheid
if(fabs(speed)>1E-2)
{
bending=-bending_factor*(turn_speed/speed);
}
const float desired_pitch = clip(atan2f(pitch_factor * velocity.y(), sqrt(velocity.x() * velocity.x() + velocity.z() * velocity.z())),
min_pitch,
max_pitch);
pitch = StepTowards(pitch, desired_pitch, static_cast<float>(pitch_change_speed * dt));
/// stabielheid dingen:
myWaitTime -= dt;
if (myWaitTime <= 0.)
{
myWaitTime = (20. + 20. * my_random());
newGoal();
}
}
//#include <osgGA/TrackballManipulator>
osg::Vec3f toOSGVector(Eigen::Vector3f v) { return osg::Vec3f(v.x(), v.y(), v.z());}
DWORD BuildCubesVert::runThread() {
int i = cubeIndex;
Eigen::Vector3f center = Eigen::Vector3f(
(*mCubes)[i].X + (*mCubes)[i].Width/2.f,
(*mCubes)[i].Y + (*mCubes)[i].Height/2.f,
(*mCubes)[i].Z + (*mCubes)[i].Depth/2.f);
int inside = 0;
bool addFace[6];
memset(addFace, true, 6);
Eigen::Vector3f points[6];
points[0] = center + Eigen::Vector3f((*mCubes)[i].Width, 0.f, 0.f);
points[1] = center - Eigen::Vector3f((*mCubes)[i].Width, 0.f, 0.f);
points[2] = center + Eigen::Vector3f(0.f, (*mCubes)[i].Height, 0.f);
points[3] = center - Eigen::Vector3f(0.f, (*mCubes)[i].Height, 0.f);
points[4] = center + Eigen::Vector3f(0.f, 0.f, (*mCubes)[i].Depth);
points[5] = center - Eigen::Vector3f(0.f, 0.f, (*mCubes)[i].Depth);
for (unsigned int p = 0; p < 6; p++) {
kdtreeNode target;
target.xyz[0] = points[p].x();
target.xyz[1] = points[p].y();
target.xyz[2] = points[p].z();
std::pair<treeType::const_iterator,double> found = kdCubes->find_nearest(target);
int k = found.first->index;
//add the min/max vertex to the list
Eigen::Vector3f t_min = Eigen::Vector3f(
(*mCubes)[k].X,
(*mCubes)[k].Y,
(*mCubes)[k].Z);
Eigen::Vector3f t_max = Eigen::Vector3f(
(*mCubes)[k].X + (*mCubes)[k].Width,
(*mCubes)[k].Y + (*mCubes)[k].Height,
(*mCubes)[k].Z + (*mCubes)[k].Depth);
for (unsigned int p = 0; p < 6; p++) {
if (points[p].x() > t_min.x() &&
points[p].y() > t_min.y() &&
points[p].z() > t_min.z() &&
points[p].x() < t_max.x() &&
points[p].y() < t_max.y() &&
points[p].z() < t_max.z()) {
addFace[p] = false;
inside++;
}
}
}
/*
for (unsigned int k = 0; k < (*mCubes).size(); k++) {
//add the min/max vertex to the list
Eigen::Vector3f t_min = Eigen::Vector3f(
(*mCubes)[k].X,
(*mCubes)[k].Y,
(*mCubes)[k].Z);
Eigen::Vector3f t_max = Eigen::Vector3f(
(*mCubes)[k].X + (*mCubes)[k].Width,
(*mCubes)[k].Y + (*mCubes)[k].Height,
(*mCubes)[k].Z + (*mCubes)[k].Depth);
for (unsigned int p = 0; p < 6; p++) {
if (points[p].x() > t_min.x() &&
points[p].y() > t_min.y() &&
points[p].z() > t_min.z() &&
points[p].x() < t_max.x() &&
points[p].y() < t_max.y() &&
points[p].z() < t_max.z()) {
addFace[p] = false;
inside++;
}
}
if (inside >= 6)
break;
}
*/
if (inside < 6) {
lock();
//add the min/max vertex to the list
Eigen::Vector3f min = Eigen::Vector3f(
(*mCubes)[i].X,
(*mCubes)[i].Y,
(*mCubes)[i].Z);
Eigen::Vector3f max = Eigen::Vector3f(
(*mCubes)[i].X + (*mCubes)[i].Width,
(*mCubes)[i].Y + (*mCubes)[i].Height,
(*mCubes)[i].Z + (*mCubes)[i].Depth);
// Create Vertices
float3 verts[8] = {
float3(min.x(), min.y(), max.z()),
float3(max.x(), min.y(), max.z()),
float3(max.x(), max.y(), max.z()),
float3(min.x(), max.y(), max.z()),
float3(min.x(), min.y(), min.z()),
float3(max.x(), min.y(), min.z()),
float3(max.x(), max.y(), min.z()),
float3(min.x(), max.y(), min.z()),
};
// Add Indices
unsigned int startIndex = mVertices->size();
// Right
if (addFace[0]) {
for (unsigned int index = 0; index < 6; index++) {
mIndices->push_back(VolumeIndicesRight[index] + startIndex);
}
}
// Left
if (addFace[1]) {
for (unsigned int index = 0; index < 6; index++) {
mIndices->push_back(VolumeIndicesLeft[index] + startIndex);
}
}
// Back
if (addFace[2]) {
for (unsigned int index = 0; index < 6; index++) {
mIndices->push_back(VolumeIndicesBack[index] + startIndex);
}
}
// Front
if (addFace[3]) {
for (unsigned int index = 0; index < 6; index++) {
mIndices->push_back(VolumeIndicesFront[index] + startIndex);
}
}
// Top
if (addFace[4]) {
for (unsigned int index = 0; index < 6; index++) {
mIndices->push_back(VolumeIndicesTop[index] + startIndex);
}
}
// Bottom
if (addFace[5]) {
for (unsigned int index = 0; index < 6; index++) {
mIndices->push_back(VolumeIndicesBottom[index] + startIndex);
}
}
// Add Vertices
for (unsigned int index = 0; index < 8; index++) {
mVertices->push_back(VertexPositionColor(verts[index], verts[index]));
}
unlock();
}
return 0;
}
Eigen::Vector2f multiplyPointMatrix(Eigen::MatrixXf h, float x, float y)
{
Eigen::Vector3f p(x, y, 1.0f);
Eigen::Vector3f r = h * p;
return Eigen::Vector2f(r.x() / r.z(), r.y() / r.z());
}
pcl::PointCloud<pcl::PointNormal>::Ptr CloudFactory::createCylinderSection(const float _angle, const float _radius, const float _height, const Eigen::Vector3f &_center, const int _npoints)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>());
float ratio = _radius / _height;
int Nside = sqrt(_npoints * (ratio / (1 + ratio)));
int Ncover = sqrt(_npoints * (1 / (1 + ratio)));
double minZ = _center.z() - _height * 0.5;
double maxZ = _center.z() + _height * 0.5;
double stepZ = _height / Nside;
double stepAngle = _angle / Ncover;
double stepRadius = _radius * 0.02;
// Generate cylinder top and bottom
for (double angle = 0; angle < 2 * M_PI; angle += stepAngle)
{
for (double r = _radius; r > 0; r -= stepRadius)
{
cloud->push_back(PointFactory::createPointNormal(r * cos(angle) + _center.x(), r * sin(angle) + _center.y(), minZ, 0, 0, -1));
cloud->push_back(PointFactory::createPointNormal(r * cos(angle) + _center.x(), r * sin(angle) + _center.y(), maxZ, 0, 0, 1));
}
}
// Generate cylinder side
for (double z = minZ; z <= maxZ; z += stepZ)
{
for (double angle = 0; angle < 2 * M_PI; angle += stepAngle)
{
Eigen::Vector3f normal = Eigen::Vector3f(_radius * cos(angle), _radius * sin(angle), 0).normalized();
cloud->push_back(PointFactory::createPointNormal(_radius * cos(angle) + _center.x(), _radius * sin(angle) + _center.y(), z, normal.x(), normal.y(), normal.z()));
}
}
return cloud;
}
// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void
_psmove_orientation_fusion_madgwick_marg_update(
PSMoveOrientation *orientation_state,
float delta_t,
const Eigen::Vector3f ¤t_omega,
const Eigen::Vector3f ¤t_g,
const Eigen::Vector3f ¤t_m)
{
// If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update
if (current_g.isZero(k_normal_epsilon) || current_m.isZero(k_normal_epsilon))
{
_psmove_orientation_fusion_imu_update(
orientation_state,
delta_t,
current_omega,
current_g);
return;
}
PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;
// Current orientation from earth frame to sensor frame
Eigen::Quaternionf SEq = orientation_state->quaternion;
// Get the direction of the magnetic fields in the identity pose.
// NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46)
// but since we've already done the work in calibration to get this vector, let's just use it.
// This also removes the last assumption in this function about what
// the orientation of the identity-pose is (handled by the sensor transform).
PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
Eigen::Vector3f k_identity_m_direction = Eigen::Vector3f(identity_m.x, identity_m.y, identity_m.z);
// Get the direction of the gravitational fields in the identity pose
PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);
// Eqn 15) Applied to the gravity and magnetometer vectors
// Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g|
// |f_b|
Eigen::Matrix<float, 3, 1> f_g;
psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);
Eigen::Matrix<float, 3, 1> f_m;
psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL);
Eigen::Matrix<float, 6, 1> f_gb;
f_gb.block<3, 1>(0, 0) = f_g;
f_gb.block<3, 1>(3, 0) = f_m;
// Eqn 21) Applied to the gravity and magnetometer vectors
// Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b]
Eigen::Matrix<float, 4, 3> J_g;
psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);
Eigen::Matrix<float, 4, 3> J_m;
psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m);
Eigen::Matrix<float, 4, 6> J_gb;
J_gb.block<4, 3>(0, 0) = J_g; J_gb.block<4, 3>(0, 3) = J_m;
// Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm)
// Compute the gradient of the objective function
Eigen::Matrix<float, 4, 1> gradient_f = J_gb*f_gb;
Eigen::Quaternionf SEqHatDot =
Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));
// normalize the gradient to estimate direction of the gyroscope error
psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);
// Eqn 47) omega_err= 2*SEq*SEqHatDot
// compute angular estimated direction of the gyroscope error
Eigen::Quaternionf omega_err = Eigen::Quaternionf(SEq.coeffs()*2.f) * SEqHatDot;
// Eqn 48) net_omega_bias+= zeta*omega_err
// Compute the net accumulated gyroscope bias
marg_state->omega_bias = Eigen::Quaternionf(marg_state->omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_t);
marg_state->omega_bias.w() = 0.f; // no bias should accumulate on the w-component
// Eqn 49) omega_corrected = omega - net_omega_bias
Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
Eigen::Quaternionf corrected_omega = Eigen::Quaternionf(omega.coeffs() - marg_state->omega_bias.coeffs());
// Compute the rate of change of the orientation purely from the gyroscope
// Eqn 12) q_dot = 0.5*q*omega
Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) * corrected_omega;
// Compute the estimated quaternion rate of change
// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);
// Compute then integrate the estimated quaternion rate
// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t);
// Make sure the net quaternion is a pure rotation quaternion
SEq_new.normalize();
// Save the new quaternion back into the orientation state
orientation_state->quaternion = SEq_new;
}
std::vector<mavros_msgs::Mavlink> HilSensorLevelInterface::CollectData() {
boost::mutex::scoped_lock lock(mtx_);
ros::Time current_time = ros::Time::now();
uint64_t time_usec = RosTimeToMicroseconds(current_time);
mavlink_message_t mmsg;
std::vector<mavros_msgs::Mavlink> hil_msgs;
// Rotate gyroscope, accelerometer, and magnetometer data into NED frame
Eigen::Vector3f gyro = R_S_B_ * hil_data_.gyro_rad_per_s;
Eigen::Vector3f acc = R_S_B_ * hil_data_.acc_m_per_s2;
Eigen::Vector3f mag = R_S_B_ * hil_data_.mag_G;
// Check if we need to publish a HIL_GPS message.
if ((current_time.nsec - last_gps_pub_time_nsec_) >= gps_interval_nsec_) {
last_gps_pub_time_nsec_ = current_time.nsec;
// Rotate ground speed data into NED frame
Eigen::Vector3i gps_vel = (R_S_B_ * hil_data_.gps_vel_cm_per_s.cast<float>()).cast<int>();
// Fill in a MAVLINK HIL_GPS message and convert it to MAVROS format.
hil_gps_msg_.time_usec = time_usec;
hil_gps_msg_.fix_type = hil_data_.fix_type;
hil_gps_msg_.lat = hil_data_.lat_1e7deg;
hil_gps_msg_.lon = hil_data_.lon_1e7deg;
hil_gps_msg_.alt = hil_data_.alt_mm;
hil_gps_msg_.eph = hil_data_.eph_cm;
hil_gps_msg_.epv = hil_data_.epv_cm;
hil_gps_msg_.vel = hil_data_.vel_1e2m_per_s;
hil_gps_msg_.vn = gps_vel.x();
hil_gps_msg_.ve = gps_vel.y();
hil_gps_msg_.vd = gps_vel.z();
hil_gps_msg_.cog = hil_data_.cog_1e2deg;
hil_gps_msg_.satellites_visible = hil_data_.satellites_visible;
mavlink_hil_gps_t* hil_gps_msg_ptr = &hil_gps_msg_;
mavlink_msg_hil_gps_encode(1, 0, &mmsg, hil_gps_msg_ptr);
mavros_msgs::MavlinkPtr rmsg_hil_gps = boost::make_shared<mavros_msgs::Mavlink>();
rmsg_hil_gps->header.stamp.sec = current_time.sec;
rmsg_hil_gps->header.stamp.nsec = current_time.nsec;
mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_gps);
hil_msgs.push_back(*rmsg_hil_gps);
}
// Fill in a MAVLINK HIL_SENSOR message and convert it to MAVROS format.
hil_sensor_msg_.time_usec = time_usec;
hil_sensor_msg_.xacc = acc.x();
hil_sensor_msg_.yacc = acc.y();
hil_sensor_msg_.zacc = acc.z();
hil_sensor_msg_.xgyro = gyro.x();
hil_sensor_msg_.ygyro = gyro.y();
hil_sensor_msg_.zgyro = gyro.z();
hil_sensor_msg_.xmag = mag.x();
hil_sensor_msg_.ymag = mag.y();
hil_sensor_msg_.zmag = mag.z();
hil_sensor_msg_.abs_pressure = hil_data_.pressure_abs_mBar;
hil_sensor_msg_.diff_pressure = hil_data_.pressure_diff_mBar;
hil_sensor_msg_.pressure_alt = hil_data_.pressure_alt;
hil_sensor_msg_.temperature = hil_data_.temperature_degC;
hil_sensor_msg_.fields_updated = kAllFieldsUpdated;
mavlink_hil_sensor_t* hil_sensor_msg_ptr = &hil_sensor_msg_;
mavlink_msg_hil_sensor_encode(1, 0, &mmsg, hil_sensor_msg_ptr);
mavros_msgs::MavlinkPtr rmsg_hil_sensor = boost::make_shared<mavros_msgs::Mavlink>();
rmsg_hil_sensor->header.stamp.sec = current_time.sec;
rmsg_hil_sensor->header.stamp.nsec = current_time.nsec;
mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_sensor);
hil_msgs.push_back(*rmsg_hil_sensor);
return hil_msgs;
}
static void
_psmove_orientation_fusion_complementary_marg_update(
PSMoveOrientation *orientation_state,
float delta_t,
const Eigen::Vector3f ¤t_omega,
const Eigen::Vector3f ¤t_g,
const Eigen::Vector3f ¤t_m)
{
// TODO: Following variable is unused
PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;
// Get the direction of the magnetic fields in the identity pose
PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
Eigen::Vector3f k_identity_m_direction = Eigen::Vector3f(identity_m.x, identity_m.y, identity_m.z);
// Get the direction of the gravitational field in the identity pose
PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);
// Angular Rotation (AR) Update
//-----------------------------
// Compute the rate of change of the orientation purely from the gyroscope
// q_dot = 0.5*q*omega
Eigen::Quaternionf q_omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
Eigen::Quaternionf q_derivative = Eigen::Quaternionf(orientation_state->quaternion.coeffs()*0.5f) * q_omega;
// Integrate the rate of change to get a new orientation
// q_new= q + q_dot*dT
Eigen::Quaternionf q_step = Eigen::Quaternionf(q_derivative.coeffs() * delta_t);
Eigen::Quaternionf ar_orientation = Eigen::Quaternionf(orientation_state->quaternion.coeffs() + q_step.coeffs());
// Make sure the resulting quaternion is normalized
ar_orientation.normalize();
// Magnetic/Gravity (MG) Update
//-----------------------------
const Eigen::Vector3f* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction };
const Eigen::Vector3f* mg_to[2] = { ¤t_g, ¤t_m };
Eigen::Quaternionf mg_orientation;
bool mg_align_success =
psmove_alignment_quaternion_between_vector_frames(
mg_from, mg_to, 0.1f, orientation_state->quaternion, mg_orientation);
// Blending Update
//----------------
if (mg_align_success)
{
// The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame
float mg_wight = orientation_state->fusion_state.complementary_marg_state.mg_weight;
orientation_state->quaternion =
psmove_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight);
// Update the blend weight
orientation_state->fusion_state.complementary_marg_state.mg_weight =
lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f);
}
else
{
orientation_state->quaternion = ar_orientation;
}
}
Eigen::Matrix4f translate(const Eigen::Vector3f& v)
{
return translate(v.x(),v.y(),v.z());
}
