void Camera::rotateCamera(int i){
float ang = 3.0*i;
AngleAxis<float> a((ang*M_PI)/180.0, Vector3f::UnitY() );
Matrix3f m;
Vector3f v1, v2, v3;
Vector3f eyeOld;
float dist;
eyeOld = eye;
// Define axis of rotation
m = AngleAxisf((0.0*M_PI)/180.0, Vector3f::UnitX())
* AngleAxisf((ang*M_PI)/180.0, Vector3f::UnitY())
* AngleAxisf((0.0*M_PI)/180.0, Vector3f::UnitZ());
// Find vector from eye to aim and use as rotation point around origin
v1 << aim - eye;
dist = v1.norm();
// Rotate point and find vector between the original point and the rotated point
v2 << v1;
v2 << m * v2;
v2 << v2 - v1;
// Apply the difference to the eye
v3 << eye + v2;
v3 << v3 - aim;
v3.normalize();
eye << dist * v3 + aim;
eye(1) = eyeOld(1);
setCamera();
}
Vector3f GSphere::GetSamplePoint(int n, int count)const
{
int belt = sqrt((double)count) + 1;
if(belt < 3) belt = 3;
while(count % belt !=0) ++belt;
int rings = count / belt;
int ring_count = n / belt;
int belt_count = n % belt;
float pitch_radius = M_PI / (rings + 1.0f);
float yaw_radius = M_PI / belt;
float pitch = 2* pitch_radius * (ring_count + 1) - M_PI + random(-pitch_radius, pitch_radius);
float yaw = 2* yaw_radius * belt_count + random(-yaw_radius, yaw_radius);
return m_position + AngleAxisf(pitch, Vector3f::UnitX()) * AngleAxisf(yaw, Vector3f::UnitY()) * Vector3f(0,0,m_radius);
}
void UpdateCoords(vector<Vec3f>& v, Vec3f position, Vec3f velocity, Vec4f rotation, Vec4f wvelocity, float dt, float scale, bool pers, float inter, float xScale, float yScale, float zFar, float zNear){
float angle = rotation[0];
angle += wvelocity[0]*dt*inter;
Vector3f newPos = Vector3f(position[0] + velocity[0]*dt*inter, position[1] + velocity[1]*dt*inter, position[2] + velocity[2]*dt*inter);
Vector3f axis(rotation[1], rotation[2], rotation[3]);
axis.normalize();
Translation3f move =Translation3f(newPos.x(), newPos.y(), newPos.z());
angle = angle*2*M_PI/360;
AngleAxisf turn = AngleAxisf(angle, axis);
Matrix4f pmat;
if(pers){
pmat << xScale, 0, 0, 0,
0, yScale, 0, 0,
0, 0, -(zFar+zNear)/(zFar-zNear), -1,
0, 0, -2*zNear*zFar/(zFar-zNear), 0;
}else{
pmat = Matrix4f::Identity();
}
for(unsigned int boxV = 0; boxV < v.size(); boxV++){
Vector3f p = Vector3f(v[boxV][0], v[boxV][1], v[boxV][2]);
/*Vector4f pt(p.x(), p.y(), p.z(), 1);
pt = pmat*pt;
p = Vector3f(pt.x(), pt.y(), pt.z());*/
p = scale* p;
if(axis.norm() > 0)
p = turn* p;
p = move* p;
v[boxV] = Vec3f(p.x(), p.y(), p.z());
}
}
Affine3f Arm::getAngleAxis(Vector3f v) {
if (v == Vector3f(0,0,0)) {
return Affine3f::Identity();
}
float mag = v.norm();
v.normalize();
return (Affine3f) AngleAxisf(mag, v);
}
void GetTransformedPoint(Vec3f& pt, Vec3f position, Vec3f velocity, Vec4f rotation, Vec4f wvelocity, float dt, float scale = 1.f, float inter = 1.0){
float angle = rotation[0];
angle += wvelocity[0]*dt*inter;
Vector3f newPos = Vector3f(position[0] + velocity[0]*dt*inter, position[1] + velocity[1]*dt*inter, position[2] + velocity[2]*dt*inter);
Vector3f axis(rotation[1], rotation[2], rotation[3]);
axis.normalize();
Translation3f move =Translation3f(newPos.x(), newPos.y(), newPos.z());
angle = angle*2*M_PI/360;
AngleAxisf turn = AngleAxisf(angle, axis);
Vector3f p = Vector3f(pt[0], pt[1], pt[2]);
p = p*scale;
p = turn*p;
p = move*p;
pt = Vec3f(p.x(), p.y(), p.z());
}
Matrix<float, 1, 3> Arm::compute_jacovian_segment(int seg_num, Point3f goal_point, Vector3f angle) {
Segment *s = segments[seg_num];
// mini is the amount of angle you go in the direction for numerical calculation
float mini = 0.0005;
Point3f transformed_goal = goal_point;
for(int i=segments.size()-1; i>seg_num; i--) {
// transform the goal point to relevence to this segment
// by removing all the transformations the segments afterwards
// apply on the current segment
transformed_goal -= segments[i]->get_end_point();
}
Point3f my_end_effector = calculate_end_effector(seg_num);
// transform them both to the origin
if (seg_num-1 >= 0) {
my_end_effector -= calculate_end_effector(seg_num-1);
transformed_goal -= calculate_end_effector(seg_num-1);
}
// original end_effector
Point3f original_ee = calculate_end_effector();
// angle input is the one you rotate around
// remove all the rotations from the previous segments by applying them
AngleAxisf t = AngleAxisf(mini, angle);
// transform the segment by some delta(theta)
s->transform(t);
// new end_effector
Point3f new_ee = calculate_end_effector();
// reverse the transformation afterwards
s->transform(t.inverse());
// difference between the end_effectors
// since mini is very small, it's an approximation of
// the derivative when divided by mini
Vector3f diff = new_ee - original_ee;
// return the row of dx/dtheta, dy/dtheta, dz/dtheta
Matrix<float, 1, 3> ret;
ret << diff[0]/mini, diff[1]/mini, diff[2]/mini;
return ret;
}
GTEST_TEST(RotationMatrix, normalize)
{
for(int i = 0; i < RUNS; ++i)
{
const Vector3f rVec = Vector3f::Random();
const AngleAxisf aa = AngleAxisf(Random::uniform(-pi, pi), Vector3f::Random().normalized());
const RotationMatrix r(aa);
const RotationMatrix scaledR = RotationMatrix(Vector3f(Vector3f::Random()).asDiagonal()) * r;
const Vector3f r1 = r * rVec;
const Vector3f r2 = r.normalized() * rVec;
EXPECT_TRUE(r1.isApprox(r2))
<< "r1:\n"
<< r1 << "\n"
<< "r2\n"
<< r2 << "\n";
}
}
GTEST_TEST(RotationMatrix, rotateY)
{
for(int i = 0; i < RUNS; ++i)
{
const Vector3f rVec = Vector3f::Random();
const float rot = Random::uniform(-pi, pi);
const AngleAxisf aa = AngleAxisf(Random::uniform(-pi, pi), Vector3f::Random().normalized());
const RotationMatrix q = aa * Rotation::aroundY(rot);
const RotationMatrix r = RotationMatrix(aa).rotateY(rot);
const Vector3f r1 = q * rVec;
const Vector3f r2 = r * rVec;
EXPECT_TRUE(r1.isApprox(r2))
<< "r1:\n"
<< r1 << "\n"
<< "r2\n"
<< r2 << "\n";
}
}
GTEST_TEST(RotationMatrix, getPackedAngleAxisFaulty)
{
Vector3f vec(1, 2, 3);
AngleAxisf aa(pi - 0.0001f, Vector3f::UnitX());
RotationMatrix r = aa;
Vector3f r1 = aa * vec;
Vector3f r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(pi - 0.0001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(pi - 0.0001f, Vector3f::UnitZ());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.0001f, Vector3f::UnitX());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.0001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.0001f, Vector3f::UnitZ());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-3.14060259f, Vector3f(-0.496929348f, 0.435349584f, 0.750687659f));
AngleAxisf aa3 = Rotation::AngleAxis::unpack(r.getPackedAngleAxis());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
}
Matrix3f m; m = AngleAxisf(0.25*M_PI, Vector3f::UnitX()) * AngleAxisf(0.5*M_PI, Vector3f::UnitY()) * AngleAxisf(0.33*M_PI, Vector3f::UnitZ()); cout << m << endl << "is unitary: " << m.isUnitary() << endl;
GTEST_TEST(RotationMatrix, getAngleAxis)
{
Vector3f vec(1, 2, 3);
AngleAxisf aa(pi, Vector3f::UnitX());
RotationMatrix r = aa;
Vector3f r1 = aa * vec;
Vector3f r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(pi - 0.000001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(pi, Vector3f::UnitZ());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi, Vector3f::UnitX());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.00001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi, Vector3f::UnitZ());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-3.14060259f, Vector3f(-0.496929348f, 0.435349584f, 0.750687659f));
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
if(!r1.isApprox(r2, 1e-4f))
EXPECT_TRUE(r1.isApprox(r2, 1e-4f));
aa = AngleAxisf::Identity();
r = RotationMatrix::Identity() * 0.99999f;
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
if(!r1.isApprox(r2))
EXPECT_TRUE(r1.isApprox(r2));
for(int i = 0; i < RUNS; ++i)
{
vec = Vector3f::Random();
r = aa = AngleAxisf(Random::uniform(-pi, pi), Vector3f::Random().normalized());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
if(!r1.isApprox(r2, 1e-2f))
EXPECT_TRUE(r1.isApprox(r2, 1e-2f))
<< "r1:\n"
<< r1 << "\n"
<< "r2\n"
<< r2 << "\n";
}
}
void InertialDataFilter::update(InertialData& inertialData)
{
DECLARE_PLOT("module:InertialDataFilter:expectedAccX");
DECLARE_PLOT("module:InertialDataFilter:accX");
DECLARE_PLOT("module:InertialDataFilter:expectedAccY");
DECLARE_PLOT("module:InertialDataFilter:accY");
DECLARE_PLOT("module:InertialDataFilter:expectedAccZ");
DECLARE_PLOT("module:InertialDataFilter:accZ");
// check whether the filter shall be reset
if(!lastTime || theFrameInfo.time <= lastTime)
{
if(theFrameInfo.time == lastTime)
return; // weird log file replaying?
reset();
}
if(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
{
reset();
}
// get foot positions
Pose3f leftFoot = theRobotModel.limbs[Limbs::footLeft];
Pose3f rightFoot = theRobotModel.limbs[Limbs::footRight];
leftFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
rightFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
const Pose3f leftFootInvert(leftFoot.inverse());
const Pose3f rightFootInvert(rightFoot.inverse());
// calculate rotation and position offset using the robot model (joint data)
const Pose3f leftOffset(lastLeftFoot.translation.z() != 0.f ? Pose3f(lastLeftFoot).conc(leftFootInvert) : Pose3f());
const Pose3f rightOffset(lastRightFoot.translation.z() != 0.f ? Pose3f(lastRightFoot).conc(rightFootInvert) : Pose3f());
// detect the foot that is on ground
bool useLeft = true;
if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x() != 0)
{
useLeft = theWalkingEngineOutput.speed.translation.x() > 0 ?
(leftOffset.translation.x() > rightOffset.translation.x()) :
(leftOffset.translation.x() < rightOffset.translation.x());
}
else
{
Pose3f left(mean.rotation);
Pose3f right(mean.rotation);
left.conc(leftFoot);
right.conc(rightFoot);
useLeft = left.translation.z() < right.translation.z();
}
// update the filter
const float timeScale = theFrameInfo.cycleTime;
predict(RotationMatrix::fromEulerAngles(theInertialSensorData.gyro.x() * timeScale,
theInertialSensorData.gyro.y() * timeScale, 0));
// insert calculated rotation
safeRawAngle = theInertialSensorData.angle.head<2>().cast<float>();
bool useFeet = true;
MODIFY("module:InertialDataFilter:useFeet", useFeet);
if(useFeet &&
(theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::standHigh)) &&
std::abs(safeRawAngle.x()) < calculatedAccLimit.x() && std::abs(safeRawAngle.y()) < calculatedAccLimit.y())
{
const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
Vector3f accGravOnly(usedRotation.col(0).z(), usedRotation.col(1).z(), usedRotation.col(2).z());
accGravOnly *= Constants::g_1000;
readingUpdate(accGravOnly);
}
else // insert acceleration sensor values
readingUpdate(theInertialSensorData.acc);
// fill the representation
inertialData.angle = Vector2a(std::atan2(mean.rotation.col(1).z(), mean.rotation.col(2).z()), std::atan2(-mean.rotation.col(0).z(), mean.rotation.col(2).z()));
inertialData.acc = theInertialSensorData.acc;
inertialData.gyro = theInertialSensorData.gyro;
inertialData.orientation = Rotation::removeZRotation(Quaternionf(mean.rotation));
// this keeps the rotation matrix orthogonal
mean.rotation.normalize();
// store some data for the next iteration
lastLeftFoot = leftFoot;
lastRightFoot = rightFoot;
lastTime = theFrameInfo.time;
// plots
Vector3f angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(inertialData.orientation));
PLOT("module:InertialDataFilter:angleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:angleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:angleZ", toDegrees(angleAxisVec.z()));
PLOT("module:InertialDataFilter:unfilteredAngleX", theInertialSensorData.angle.x().toDegrees());
PLOT("module:InertialDataFilter:unfilteredAngleY", theInertialSensorData.angle.y().toDegrees());
angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(mean.rotation));
PLOT("module:InertialDataFilter:interlanAngleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:interlanAngleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:interlanAngleZ", toDegrees(angleAxisVec.z()));
}
void Segment::randomize() {
// randomize
T = AngleAxisf(PI/2, Vector3f::Random()) * T;
}
int main(int argc, char **argv)
{
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<> dis(-1, 1);
// AX = ZB
Eigen::Matrix4f Z = Eigen::Matrix4f::Identity();
Eigen::Vector4f qz_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
Eigen::Quaternionf qz( qz_use[0], qz_use[1], qz_use[2], qz_use[3] );
qz.normalize();
Eigen::Vector3f tz = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
Z.block<3,3>(0,0) = qz.toRotationMatrix();
Z.block<3,1>(0,3) = tz;
//std::cout << "Z: " << std::endl << Z << std::endl;
Eigen::Matrix4f X = Eigen::Matrix4f::Identity();
Eigen::Vector4f qx_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
Eigen::Quaternionf qx( qx_use[0], qx_use[1], qx_use[2], qx_use[3] );
qx.normalize();
Eigen::Vector3f tx = Eigen::Vector3f(dis(gen),dis(gen), dis(gen));
X.block<3,3>(0,0) = qx.toRotationMatrix();
X.block<3,1>(0,3) = tx;
//std::cout << "X: " << std::endl << X << std::endl;
int NPoses = 6;
std::vector<Matrix4f> A, B;
Eigen::Matrix4f a = Eigen::Matrix4f::Identity();
Eigen::Vector4f q_use = Eigen::Vector4f::Random();
Eigen::Quaternionf q( q_use[0], q_use[1], q_use[2], q_use[3]);
q.normalize();
Eigen::Vector3f t = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
a.block<3,3>(0,0) = q.toRotationMatrix();
a.block<3,1>(0,3) = t;
for (int i = 0; i < NPoses; i++)
{
// test data
Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
b = Z.inverse()*a*X;
Eigen::Matrix4f d = Eigen::Matrix4f::Identity();
Eigen::Vector3f t = 0.001*Eigen::Vector3f( dis(gen),dis(gen), dis(gen) );
float roll = dis(gen)*M_PI/512;
float pitch = dis(gen)*M_PI/512;
float yaw = dis(gen)*M_PI/512;
Eigen::Matrix3f R;
R = AngleAxisf(roll, Vector3f::UnitX())
*AngleAxisf(pitch, Vector3f::UnitY())
*AngleAxisf(yaw, Vector3f::UnitZ());
d.block<3,3>(0,0) = R;
d.block<3,1>(0,3) = t;
a = d*a;
A.push_back(a);
B.push_back(b);
}
// first, create an object of your class
calibration::Calibration mycalib;
mycalib.setInput(A, B);
mycalib.computeNonLinOpt();
Eigen::Matrix4f Tx, Tz;
Tx = Eigen::Matrix4f::Identity();
Tz = Eigen::Matrix4f::Identity();
mycalib.getOutput(Tz, Tx);
// print out the result
std::cout << "Input Z: " << std::endl << Z << std::endl;
std::cout << "Estimated Z:" << std::endl << Tz << std::endl;
std::cout << "Error over Z estimation: " << std::endl << Z-Tz << std::endl << std::endl;
std::cout << "Input X: " << std::endl << X << std::endl;
std::cout << "Estimated X:" << std::endl << Tx << std::endl;
std::cout << "Error over X estimation: " << std::endl << X-Tx << std::endl;
return 0;
}
__device__ __host__ __forceinline__ void Rodrigues(const float3& rvec, float3& row1, float3& row2, float3& row3)
{
float angle = norm(rvec);
float3 unit_axis = make_float3(rvec.x/angle, rvec.y/angle, rvec.z/angle);
AngleAxisf(angle, unit_axis, row1, row2, row3);
}
void Segment::apply_angle_change(float rad_change, Vector3f angle) {
T = AngleAxisf(rad_change, angle) * T;
}
SceneObject& SceneObject::rotate(float angle, const Vector3f& axis) {
rotate(Quaternion<float>(AngleAxisf(angle, axis)));
return *this;
}
system::error_code AttitudeControlSimple::update(asio::yield_context yctx)
{
Mat3x3f R = m_ISAttitudeEstimated.m_Value.toRotationMatrix();
#if 1
Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
#else
#if 0
Mat3x3f R_sp = Quaternionf(AngleAxisf(M_PI_4, Vec3f::UnitX())).toRotationMatrix();
#else
static once_flag initSetpoint;
call_once(initSetpoint, [this]() { m_ISAttitudeSetpoint.m_Value = m_ISAttitudeEstimated.m_Value; } );
Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
#endif
#endif
float dT = std::min(std::max(m_ISDT.m_Value, 0.002f), 0.02f);
Vec3f R_z(R(0, 2), R(1, 2), R(2, 2));
Vec3f R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
Vec3f e_R = R.transpose() * (R_z.cross(R_sp_z));
float e_R_z_sin = e_R.norm();
float e_R_z_cos = R_z.dot(R_sp_z);
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
Mat3x3f R_rp;
if(e_R_z_sin > 0.0f)
{
float e_R_z_angle = std::atan2(e_R_z_sin, e_R_z_cos);
Vec3f e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_angle * e_R_z_axis;
Mat3x3f e_R_cp = Mat3x3f::Zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
R_rp = R * (Mat3x3f::Identity() + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
}
else
{
R_rp = R;
}
Vec3f R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
Vec3f R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = std::atan2(R_rp_x.cross(R_sp_x).dot(R_sp_z), R_rp_x.dot(R_sp_x)) * yaw_w;
if(e_R_z_cos < 0.0f)
{
Quaternionf q(R.transpose() * R_sp);
Vec3f e_R_d = q.vec();
e_R_d.normalize();
e_R_d *= 2.0f * std::atan2(e_R_d.norm(), q.w());
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
Vec3f const rotationRateSetpoint = m_AttitudeP.cwiseProduct(e_R);
Vec3f e_RR = rotationRateSetpoint - m_ISRotationRateMeasured.m_Value;
Vec3f rotationRateControl =
m_RotationRateP.cwiseProduct(e_RR) +
m_RotationRateD.cwiseProduct(m_RotationRateMeasuredPrev - m_ISRotationRateMeasured.m_Value) / dT +
m_RotationRateIError;
m_RotationRateMeasuredPrev = m_ISRotationRateMeasured.m_Value;
m_RotationRateSetpointPrev = rotationRateSetpoint;
m_OSRotationRateSetpoint.m_Value = rotationRateControl;
return base::makeErrorCode(base::kENoError);
}
bool MyPointCloud::alignPointClouds(std::vector<Matrix3frm>& Rcam, std::vector<Vector3f>& tcam, MyPointCloud *globalPreviousPointCloud, device::Intr& intrinsics, int globalTime) {
Matrix3frm Rprev = Rcam[globalTime - 1]; // [Ri|ti] - pos of camera, i.e.
Vector3f tprev = tcam[globalTime - 1]; // tranfrom from camera to global coo space for (i-1)th camera pose
Matrix3frm Rprev_inv = Rprev.inverse(); //Rprev.t();
device::Mat33& device_Rprev_inv = device_cast<device::Mat33> (Rprev_inv);
float3& device_tprev = device_cast<float3> (tprev);
Matrix3frm Rcurr = Rprev; // tranform to global coo for ith camera pose
Vector3f tcurr = tprev;
#pragma omp parallel for
for(int level = LEVELS - 1; level >= 0; --level) {
int iterations = icpIterations_[level];
#pragma omp parallel for
for(int iteration = 0; iteration < iterations; ++iteration) {
{
printf(" ICP level %d iteration %d", level, iteration);
pcl::ScopeTime time("");
device::Mat33& device_Rcurr = device_cast<device::Mat33> (Rcurr);
float3& device_tcurr = device_cast<float3>(tcurr);
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<float, 6, 1> b;
if(level == 2 && iteration == 0)
error_.create(rows_ * 4, cols_);
device::estimateCombined (device_Rcurr, device_tcurr, vmaps_[level], nmaps_[level], device_Rprev_inv, device_tprev, intrinsics (level),
globalPreviousPointCloud->getVertexMaps()[level], globalPreviousPointCloud->getNormalMaps()[level], distThres_, angleThres_,
gbuf_, sumbuf_, A.data (), b.data (), error_);
//checking nullspace
float det = A.determinant ();
if (fabs (det) < 1e-15 || !pcl::device::valid_host (det)) {
// printf("ICP failed at level %d, iteration %d (global time %d)\n", level, iteration, globalTime);
return (false);
} //else printf("ICP succeed at level %d, iteration %d (global time %d)\n", level, iteration, globalTime);
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b);
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f tinc = result.tail<3> ();
//compose
tcurr = Rinc * tcurr + tinc;
Rcurr = Rinc * Rcurr;
}
}
}
//save tranform
Rcam[globalTime] = Rcurr;
tcam[globalTime] = tcurr;
return (true);
}
SceneObject& SceneObject::setRotation(float angle, const Vector3f& axis) {
setRotation(AngleAxisf(angle, axis));
return *this;
}
void GCamera::CalculateFrustum()
{
Matrix3f m;
float x = geometry::Deg2Rad(m_vfov/2.0f);
float y = geometry::Deg2Rad(m_hfov/2.0f);
// calculating rotation matrix based on pitch yaw roll
Matrix3f rot;
rot = AngleAxisf(geometry::Deg2Rad(m_roll), Vector3f::UnitZ()) *
AngleAxisf(geometry::Deg2Rad(m_pitch), Vector3f::UnitX()) *
AngleAxisf(geometry::Deg2Rad(m_yaw), Vector3f::UnitY());
// calculating frustum vecs
m = AngleAxisf(x, Vector3f::UnitX())*
AngleAxisf(y, Vector3f::UnitY()) * rot;
m_frustum[0] = Vector3f::UnitZ().transpose() * m;
m = AngleAxisf(x, Vector3f::UnitX())*
AngleAxisf(-y, Vector3f::UnitY()) * rot;
m_frustum[1] = Vector3f::UnitZ().transpose() * m;
m = AngleAxisf(-x, Vector3f::UnitX())*
AngleAxisf(y, Vector3f::UnitY()) * rot;
m_frustum[2] = Vector3f::UnitZ().transpose() * m;
m = AngleAxisf(-x, Vector3f::UnitX())*
AngleAxisf(-y, Vector3f::UnitY()) * rot;
m_frustum[3] = Vector3f::UnitZ().transpose() * m;
CalculateStepVectors();
}
