Matrix3d computeTform(
const Matrix2Xd& pointsFrom, const Matrix2Xd& pointsTo,
const VectorXd& indices ) {
Matrix3d normMatFrom;
Matrix3d normMatTo;
Matrix3d T = computeSimilarity(
normalizePoints( pointsFrom, indices, normMatFrom ),
normalizePoints( pointsTo, indices, normMatTo ) );
return normMatFrom.transpose() * ( T * normMatTo.transpose().inverse() );
}
// Original code from the ROS vslam package pe3d.cpp
// uses the SVD procedure for aligning point clouds
// SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
{
using namespace Eigen;
SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");
Vector3d c0 = p0.rowwise().mean();
Vector3d c1 = p1.rowwise().mean();
Matrix3d H(Matrix3d::Zero());
// subtract out
// p0a -= c0;
// p0b -= c0;
// p0c -= c0;
// p1a -= c1;
// p1b -= c1;
// p1c -= c1;
// Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
// p1c*p0c.transpose();
for(int i = 0; i < p0.cols(); ++i)
{
H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
}
// do the SVD thang
JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
if (det < 0.0)
{
V.col(2) = V.col(2) * -1.0;
R = V * svd.matrixU().transpose();
}
Vector3d tr = c0-R.transpose()*c1; // translation
// transformation matrix, 3x4
Matrix4d tfm(Matrix4d::Identity());
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.topLeftCorner<3,3>() = R.transpose();
tfm.topRightCorner<3,1>() = tr;
return tfm;
}
boost::array<pair<Vector3d,Matrix3d >, 2 > PlaneFromConic(const Conic& c, double plane_circle_radius, const Matrix3d& K )
{
const Matrix3d Cn = K.transpose() * c.C *K;
Eigen::JacobiSVD<Matrix3d> svd(Cn, ComputeFullU );
const Matrix3d R1 = svd.matrixU();
const Vector3d l = svd.singularValues();
const double t = atan( sqrt( (l[1]-l[0]) / (l[2]-l[1]) ) );
boost::array<pair<Vector3d,Matrix3d >, 2> r;
for( int i=0; i < 2; ++i )
{
const double theta = (i*2-1)*t;
const Matrix3d R2 = RotY(theta);
const Matrix3d R = R1 * R2;
// const Matrix3d C__ = R.T() * Cn * R;
const Vector3d n = R * Vector3d(0,0,-1);
const double d = sqrt( (l[1]*l[1])/(l[0]*l[2]) ) * plane_circle_radius;
r[i].first = n.normalized() / d;
r[i].second = R;
}
return r;
}
Vector2d MSCKF::projectPoint(Vector3d feature_pose, Matrix3d R_gb, Vector3d p_gb, Vector3d p_cb)
{
Vector2d zij;
zij = cam.h(R_cb * R_gb.transpose() * (feature_pose - p_gb) + p_cb);
return zij;
}
Matrix4d inverse_se3(Matrix4d T){
Matrix4d Tinv = Matrix4d::Identity();
Matrix3d R;
Vector3d t;
t = T.block(0,3,3,1);
R = T.block(0,0,3,3);
Tinv.block(0,0,3,3) = R.transpose();
Tinv.block(0,3,3,1) = -R.transpose() * t;
return Tinv;
}
void BodyNode::updateTransform() {
mT = mJointParent->getLocalTransform();
if (mNodeParent) mW = mNodeParent->mW * mT;
else mW = mT;
// update the inertia matrix
Matrix3d R = mW.topLeftCorner(3,3);
if(mVizShape!=NULL)
mIc = R*mI*R.transpose();
}
RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
//mb=ma
//hb=R*h
//Ib = R(Ia)R' with r=0
Matrix3d R = Map<Matrix3d>(M.data);
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
}
double angle_mismatch(const Matrix3d& Q, const Matrix3d& R)
{
//std::cout << "Q\n" << Q << "\nR\n" << R << std::endl;
Matrix3d S = Q.transpose()*R;
//std::cout << "S\n: " << S << std::endl;
double thecos = (S.trace()-1.0)/2.0;
thecos = std::max( std::min ( thecos, 1.0), -1.0);
//std::cout << "thecos: " << thecos << " leads to angle: " << acos(thecos) << " * " << ( S(1,2) < 0.0 ? -1.0 : 1.0) << std::endl;
return (acos(thecos) * ( S(1,2) < 0.0 ? -1.0 : 1.0));
}
const Matrix3d VectorMath::TMatrix(const Vector3d &v)
{
double vnormsq = v.dot(v);
Matrix3d I;
I.setIdentity();
if(vnormsq == 0)
return I;
Matrix3d R = rotationMatrix(v);
return (v*v.transpose() + (R.transpose()-I)*crossProductMatrix(v))/vnormsq;
}
double Cost( const pair<Vector3d,Matrix3d >& plane, const Conic& conic, const Matrix3d& K )
{
const Matrix3d Cn = K.transpose() * conic.C *K;
const Matrix3d& R = plane.second;
const Matrix3d C_ = R.transpose() * Cn * R;
const double a = C_(0,0);
// const double b = C_[0][1] / 2.0;
const double c = C_(1,1);
const double amc = abs(a - c);
return amc / max(abs(a),abs(c));;// + b*b;
}
void force_feedback_pos_rate_control(void){
// ****************************************************************************************************************
// Force feedback when in rate control area
// ****************************************************************************************************************
// Variables
Ks = MatrixXd::Zero(3,3);
Ks(0,0) = scale[0];
Ks(1,1) = scale[1];
Ks(2,2) = scale[2];
Eigen::Vector3d omni_force = Eigen::Vector3d::Zero();
double R = 0.05;
double Fmax = 3.3; // 3.3 N es la máxima força nominal
double Kf = 60;
double Kv = 0.5;
// Displacement
dm = pm_i - pm_0;
ds = Ks * Rs.transpose() * Rm * dm;
// Omni feedback force computation
double D = sqrt( ds(0)*ds(0) + ds(1)*ds(1) );
if (D > R) omni_force = - Kf * (D - R) * (ds/D);
omni_force += - Kv*vm_i; // Damping
omni_force(2) = 0.0; // No force in vertical axis
omni_force = Rm.transpose() * Rs * omni_force; // To master reference frame
if (omni_force(0) < -Fmax) omni_force(0) = -Fmax;
if (omni_force(0) > Fmax) omni_force(0) = Fmax;
if (omni_force(1) < -Fmax) omni_force(1) = -Fmax;
if (omni_force(1) > Fmax) omni_force(1) = Fmax;
// Publish
force.x = omni_force(0);
force.y = omni_force(1);
force.z = omni_force(2);
pub_OmniForceFeedback.publish(force);
}
void Edge_V_V_GICPLandmark::linearizeOplus()
{
// std::cout << "START Edge_V_V_GICPLandmark::linearizeOplus() " << std::endl;
VertexPointXYZ* vp0 = static_cast<VertexPointXYZ*>(_vertices[0]);
VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]);
Vector3d p1 = measurement().pos1;
if (!vp0->fixed())
{
_jacobianOplusXi.block<3,3>(0,0) = -Matrix3d::Identity();
}
if (!vp1->fixed())
{
Matrix3d R1 = vp1->estimate().matrix().topLeftCorner<3,3>();
_jacobianOplusXj.block<3,3>(0,0) = R1;
_jacobianOplusXj.block<3,1>(0,3) = R1*dRidx.transpose()*p1;
_jacobianOplusXj.block<3,1>(0,4) = R1*dRidy.transpose()*p1;
_jacobianOplusXj.block<3,1>(0,5) = R1*dRidz.transpose()*p1;
}
}
void Camera::updateOthers() {
P_ /= P_.row(2).head<3>().squaredNorm();
// RQ Factorization
Matrix3d M = P_.topLeftCorner<3, 3>();
Matrix3d reverseRows;
reverseRows << 0, 0, 1, 0, 1, 0, 1, 0, 0;
const Eigen::Matrix3d qrMatrix = (reverseRows * M).transpose();
const Eigen::HouseholderQR<Matrix3d> qrDecomp(qrMatrix);
const Matrix3d matrixQ = qrDecomp.householderQ();
const Matrix3d matrixR = qrDecomp.matrixQR().triangularView<Upper>();
R_ = reverseRows * matrixQ.transpose();
K_ = reverseRows * matrixR.transpose() * reverseRows;
// Eigen doesn't require diagonal elements of R (in the QR
// decomposition) to be positive, so correct for this since the
// intrinsic matrix should have positive diagonal elements
for(int axis = 2; axis >= 0; --axis) {
if(K_(axis, axis) < 0) {
K_(axis, axis) = -(K_(axis, axis));
R_.row(axis) = -(R_.row(axis));
}
if(K_(axis, 2) < 0)
K_(axis, 2) = -K_(axis, 2);
}
orthonormalize(R_);
Kinv_ = K_.inverse();
Rinv_ = R_.transpose();
t_ = Kinv_ * P_.col(3);
C_ = -Rinv_ * t_;
updatePrincipleRay();
}
void localizerGVG::propagate(double Vm, double Wm, double dt) {
Matrix3d Fr;
Fr<<1, 0, -Vm*dt*sin(oldYaw),
0, 1, Vm*dt*cos(oldYaw),
0, 0, 1;
MatrixXd Gr(3,2);
Gr<< -dt*cos(oldYaw), 0,
-dt*sin(oldYaw), 0,
0, -dt;
P.block(0,0,3,3)=Fr*P.block(0,0,3,3)*Fr.transpose()+Gr*Qr*Gr.transpose();
CF=Fr*CF;
normalizeCovariance();
}
Matrix3d skewlog(Matrix3d M){
Matrix3d skew;
double val = (M.trace() - 1.f)/2.f;
if(val > 1.f)
val = 1.f;
else if (val < -1.f)
val = -1.f;
double theta = acos(val);
if(theta == 0.f)
skew << 0,0,0,0,0,0,0,0,0;
else
skew << (M-M.transpose())/(2.f*sin(theta))*theta;
return skew;
}
void QuaternionFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.resize(getNumVelocities(), getNumPositions());
auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
Matrix3d R = quat2rotmat(quat);
Vector4d quattilde;
Matrix<double, SPACE_DIMENSION, QUAT_SIZE> M;
Matrix<double, SPACE_DIMENSION, QUAT_SIZE> RTransposeM;
Gradient<Vector4d, QUAT_SIZE, 1>::type dquattildedquat;
if (dqdot_to_v) {
Gradient<Vector4d, QUAT_SIZE, 2>::type ddquattildedquat;
normalizeVec(quat, quattilde, &dquattildedquat, &ddquattildedquat);
auto dR = dquat2rotmat(quat);
Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type dM;
quatdot2angularvelMatrix(quat, M, &dM);
RTransposeM.noalias() = R.transpose() * M;
auto dRTranspose = transposeGrad(dR, R.rows());
auto dRTransposeM = matGradMultMat(R.transpose(), M, dRTranspose, dM);
auto dRTransposeMdquattildedquat = matGradMultMat(RTransposeM, dquattildedquat, dRTransposeM, ddquattildedquat);
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
setSubMatrixGradient<4>(*dqdot_to_v, dRTranspose, intRange<3>(3), intRange<3>(0), qdot_to_v.rows(), 3);
setSubMatrixGradient<4>(*dqdot_to_v, dRTransposeMdquattildedquat, intRange<3>(0), intRange<4>(3), qdot_to_v.rows(), 3);
}
else {
normalizeVec(quat, quattilde, &dquattildedquat);
quatdot2angularvelMatrix(quat, M);
RTransposeM.noalias() = R.transpose() * M;
}
qdot_to_v.block<3, 3>(0, 0).setZero();
qdot_to_v.block<3, 4>(0, 3).noalias() = RTransposeM * dquattildedquat;
qdot_to_v.block<3, 3>(3, 0) = R.transpose();
qdot_to_v.block<3, 4>(3, 3).setZero();
}
//==============================================================================
double State::getCoronalRightLegAngle() const
{
Matrix3d comR = getCOMFrame().linear();
Vector3d comY = comR.col(1);
Vector3d thighAxisZ = mRightThigh->getTransform().linear().col(2);
Vector3d projThighAZ = (comR.transpose() * thighAxisZ);
projThighAZ[0] = 0.0;
projThighAZ.normalize();
double angle = _getAngleBetweenTwoVectors(projThighAZ, comY);
Vector3d cross = comY.cross(projThighAZ);
if (cross[0] > 0.0)
return angle;
else
return -angle;
}
//==============================================================================
double State::getCoronalPelvisAngle() const
{
Matrix3d comR = getCOMFrame().linear();
Vector3d comY = comR.col(1);
Vector3d pelvisZ = mPelvis->getTransform().linear().col(2);
Vector3d projPelvisZ = (comR.transpose() * pelvisZ);
projPelvisZ[0] = 0.0;
projPelvisZ.normalize();
double angle = _getAngleBetweenTwoVectors(projPelvisZ, comY);
Vector3d cross = comY.cross(projPelvisZ);
if (cross[0] > 0.0)
return angle;
else
return -angle;
}
Matrix<double,Dynamic,9>
PatchFit::general_dfda (MatrixXd ql, Matrix3d rr, MatrixXd dfdql, vector<MatrixXd> dqldr)
{
MatrixXd dfdk (ql.rows(), ql.cols());
dfdk = ql.array()*ql.array();
MatrixXd dfdr (ql.rows(), ql.cols());
dfdr.col(0) = (dfdql.array()*dqldr[0].array()).rowwise().sum();
dfdr.col(1) = (dfdql.array()*dqldr[1].array()).rowwise().sum();
dfdr.col(2) = (dfdql.array()*dqldr[2].array()).rowwise().sum();
MatrixXd dfdc (ql.rows(), ql.cols());
dfdc = -dfdql*rr.transpose();
MatrixXd j (ql.rows(), 9);
j << dfdk, dfdr, dfdc;
return j;
}
JNIEXPORT jobject JNICALL Java_com_mousebird_maply_Matrix3d_transpose
(JNIEnv *env, jobject obj)
{
try
{
Matrix3dClassInfo *classInfo = Matrix3dClassInfo::getClassInfo();
Matrix3d *inst = classInfo->getObject(env,obj);
if (!inst)
return NULL;
Matrix3d matTrans = inst->transpose();
return MakeMatrix3d(env,matTrans);
}
catch (...)
{
__android_log_print(ANDROID_LOG_VERBOSE, "Maply", "Crash in Matrix3d::transpose()");
}
return NULL;
}
void ckAlign() {
srand(time(0));
Matrix3d t = Matrix3d::Random();
Matrix3d r,s;
polarDec(t,r,s);
vector<Vector3d> v1, v2;
for (int i=0; i<2; i++) {
Vector3d v(Vector3d::Random());
v1.push_back(v);
//v2.push_back(r*v);
v2.push_back(Vector3d::Random());
}
RotateAlign align(v1, v2);
Matrix3d rr = align.calc();
cout << rr << endl;
cout << r << endl;
cout << (r-rr.transpose()).norm() << endl;
align.ckt(rr);
}
Vector3d logarithm_map_so3(Matrix3d R){
Matrix3d Id3 = Matrix3d::Identity();
Vector3d w;
Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
w << 0.f, 0.f, 0.f;
double cosine = (R.trace() - 1.f)/2.f;
if(cosine > 1.f)
cosine = 1.f;
else if (cosine < -1.f)
cosine = -1.f;
double sine = sqrt(1.0-cosine*cosine);
if(sine > 1.f)
sine = 1.f;
else if (sine < -1.f)
sine = -1.f;
double theta = acos(cosine);
if( theta > 0.000001 ){
w_hat << theta*(R-R.transpose()) / (2.f*sine);
w = skewcoords(w_hat);
}
return w;
}
// translate kpts by 6DOF transformation of frame
void Frame::setTKpts(Eigen::Vector4d trans, Eigen::Quaterniond rot)
{
Vector3d tr;
tr = trans.head<3>();
// set up 3x4 transformation from kpt+disp to kpt
Matrix4d Q;
Q << 1.0, 0.0, 0.0, -cam.cx, // fy should enter in here somewhere...
0.0, 1.0, 0.0, -cam.cy,
0.0, 0.0, 0.0, cam.fx,
0.0, 0.0, 1.0/cam.tx, 0;
Matrix<double,3,4> P;
P << cam.fx, 0.0, cam.cx, 0.0,
0.0, cam.fx, cam.cy, 0.0,
0.0, 0.0, 1.0, 0.0;
// 3D point transform - inverse of frame motion
Matrix4d T;
T.setZero();
Matrix3d R = rot.toRotationMatrix();
T.block<3,3>(0,0) = R.transpose();
T.block<3,1>(0,3) = -R*tr;
T(3,3) = 1.0;
P = P*T*Q;
// go through points and set up transformed ones
tkpts.resize(kpts.size());
Vector4d v(0.0,0.0,0.0,1.0);
for (int i=0; i<(int)kpts.size(); i++)
{
Vector3d vk;
v(0) = kpts[i].pt.x;
v(1) = kpts[i].pt.y;
v(2) = disps[i];
vk = P*v;
tkpts[i].pt.x = vk(0)/vk(2);
tkpts[i].pt.y = vk(1)/vk(2);
}
}
Vector3d
SpiceRotationModel::angularVelocity(double tdbSec) const
{
double et = tdbSec;
SpiceDouble transform[6][6];
sxform_c(m_fromFrame.c_str(), m_toFrame.c_str(), et, transform);
if (!failed_c())
{
// Extract the angular velocity vector from the state transform matrix
// Rotation matrix is the top left 3x3 matrix
Matrix3d R;
R << transform[0][0], transform[0][1], transform[0][2],
transform[1][0], transform[1][1], transform[1][2],
transform[2][0], transform[2][1], transform[2][2];
// W*R is the lower left 3x3 matrix
Matrix3d WR;
WR << transform[3][0], transform[3][1], transform[3][2],
transform[4][0], transform[4][1], transform[4][2],
transform[5][0], transform[5][1], transform[5][2];
// Multiply by inverse of R (= transpose, since R is a rotation) to get
// the skew-symmetric matrix W*
Matrix3d W = WR * R.transpose();
return Vector3d(-W(1, 2), W(0, 2), -W(0, 1));
}
else
{
char errorMessage[1024];
getmsg_c("long", sizeof(errorMessage), errorMessage);
cerr << errorMessage << std::endl;
reset_c();
return Vector3d::Zero();
}
}
Conic UnmapConic( const Conic& c, const AbstractCamera& cam )
{
std::vector<Eigen::Vector2d > d;
std::vector<Eigen::Vector2d > u;
d.push_back(c.center);
d.push_back(Eigen::Vector2d(c.bbox.x1,c.bbox.y1));
d.push_back(Eigen::Vector2d(c.bbox.x1,c.bbox.y2));
d.push_back(Eigen::Vector2d(c.bbox.x2,c.bbox.y1));
d.push_back(Eigen::Vector2d(c.bbox.x2,c.bbox.y2));
for( int i=0; i<5; ++i )
u.push_back( cam.unmap(d[i]) );
// Distortion locally estimated by homography
const Matrix3d H_du = EstimateH_ba(u,d);
Conic ret;
// ret.bbox = c.bbox;
ret.C = H_du.transpose() * c.C * H_du;
ret.Dual = ret.C.inverse();
ret.center = u[0];
return ret;
}
bodyId_, controlPoint_, true);
tare = 0;
}
// Get the latest joint state information
Vector Q(model.getNumDOFs());
Vector Qd(model.getNumDOFs());
model.getLatestFullState(Q, Qd);
Vector3d bodyTranslation = RigidBodyDynamics::CalcBodyToBaseCoordinates(model.rbdlModel(), Q, bodyId_, Vector::Zero(3), false);
Matrix3d bodyRotation = RigidBodyDynamics::CalcBodyWorldOrientation(model.rbdlModel(), Q, bodyId_, false);
getJacobian(JtLoc);
xCurWorld = bodyRotation.transpose() * controlPoint_ + bodyTranslation;
Matrix3d frameRotation;
Vector3d frameTranslation;
if(frameId_ == -1) //Using fixed world frame as reference, latching immaterial.
{
// if (projection_.cols() != xCurWorld.rows())
// {
// CONTROLIT_ERROR << "Matrix size problem:\n"
// << " - frameId_ = " << frameId_ << "\n"
// << " - projection_ = " << projection_ << "\n"
// << " - xCurWorld_ = " << xCurWorld;
// }
xCurFrameProjected = projection_ * xCurWorld;
// Gets camera velocities. Also, if target not visible (i.e. estimatorOn = false) publishes output (ground truth)
// and does prediction
void camVelCB(const geometry_msgs::TwistStampedConstPtr& twist)
{
// Time
ros::Time timeStamp = twist->header.stamp;
double timeNow = timeStamp.toSec();
double delT = timeNow - lastVelTime;
lastVelTime = timeNow;
// Camera velocities, expressed in camera coordinate system
vCc << twist->twist.linear.x,twist->twist.linear.y,twist->twist.linear.z;
wGCc << twist->twist.angular.x,twist->twist.angular.y,twist->twist.angular.z;
if (!estimatorOn)
{
// Object trans w.r.t. image frame, for ground truth
Vector3d trans;
tf::StampedTransform transform;
tfl.waitForTransform("image","ugv0",timeStamp,ros::Duration(0.1));
tfl.lookupTransform("image","ugv0",timeStamp,transform);
tf::Vector3 temp_trans = transform.getOrigin();
trans << temp_trans.getX(),temp_trans.getY(),temp_trans.getZ();
// Ground truth
Vector3d x;
x << trans.segment<2>(0)/trans(2),1/trans(2);
xlast << x; // Update for optical flow
// Object rotation w.r.t. image frame, for rotating target velocities into image coordinates
try
{
Quaterniond quat;
if (deadReckoning)
{
tf::StampedTransform tfImage2World;
tf::StampedTransform tfOdom2Marker;
tfl.waitForTransform("image","world",timeStamp,ros::Duration(0.1));
tfl.lookupTransform("image","world",timeStamp,tfImage2World);
tfl.waitForTransform("ugv0/odom","ugv0/base_footprint",timeStamp,ros::Duration(0.1));
tfl.lookupTransform("ugv0/odom","ugv0/base_footprint",timeStamp,tfOdom2Marker);
tf::Quaternion temp_quat = tfImage2World.getRotation();
Quaterniond qIm2W = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
temp_quat = tfOdom2Marker.getRotation();
Quaterniond qO2M = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
quat = qIm2W*qWorld2Odom*qO2M;
}
else
{
tf::Quaternion temp_quat = transform.getRotation();
quat = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
}
// Target velocities expressed in camera coordinates
Vector3d vTc = quat*vTt;
// Update so that delT in featureCB is reasonable after switch
lastImageTime = timeNow;
// Convert to scalars to match notation in papers
double vc1 = vCc(0); double vc2 = vCc(1); double vc3 = vCc(2);
double vq1 = vTc(0); double vq2 = vTc(1); double vq3 = vTc(2);
double w1 = wGCc(0); double w2 = wGCc(1); double w3 = wGCc(2);
double x1hat = xhat(0); double x2hat = xhat(1); double x3hat = xhat(2);
// Predictor
double Omega1 = w3*x2hat - w2 - w2*pow(x1hat,2) + w1*x1hat*x2hat;
double Omega2 = w1 - w3*x1hat - w2*x1hat*x2hat + w1*pow(x2hat,2);
double xi1 = (vc3*x1hat - vc1)*x3hat;
double xi2 = (vc3*x2hat - vc2)*x3hat;
double x1hatDot = Omega1 + xi1 + vq1*x3hat - x1hat*vq3*x3hat;
double x2hatDot = Omega2 + xi2 + vq2*x3hat - x2hat*vq3*x3hat;
double x3hatDot = vc3*pow(x3hat,2) - (w2*x1hat - w1*x2hat)*x3hat - vq3*pow(x3hat,2);
// Predict Covariance
Matrix3d F = calculate_F(xhat,vCc,vTc,wGCc);
MatrixXd Pdot = F*P+P*F.transpose() + Q;
// Update states and covariance
Vector3d xhatDot;
xhatDot << x1hatDot, x2hatDot, x3hatDot;
xhat += xhatDot*delT;
P += Pdot*delT;
// Publish output
publishOutput(x,xhat,trans,timeStamp);
}
catch (tf::TransformException e)
{
}
}
}
void
basic_ins_qkf::predict_ned(const Vector3d& gyro_meas,
const Vector3d& accel_meas,
double dt)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
Vector3d accel_body = avg_state.orientation.conjugate()*accel_meas;
// Vector3d accel_dir = accel_body.normalized();
Matrix<double, 3, 3> accel_cov = cross<double>(-accel_body);
// 1500x realtime, without vectorization, on 2.2 GHz Athlon X2
// See basic_ins_qkf::predict() for the full blockwise form
const Matrix<double, 12, 12> pcov = cov;
const Matrix3d dtR = dt * avg_state.orientation.conjugate().toRotationMatrix();
const Matrix3d dtQ = accel_cov * dt;
cov.block<3, 3>(0, 3) -= pcov.block<3,3>(0, 0)*dtR.transpose();
cov.block<3, 3>(0, 6) += dt * pcov.block<3, 3>(0, 9);
cov.block<3, 3>(0, 9) -= pcov.block<3, 3>(0, 3) * dtQ.transpose();
cov.block<3, 3>(3, 3) += dtR*pcov.block<3, 3>(0, 0)*dtR.transpose()
- dtR*pcov.block<3, 3>(0, 3) - pcov.block<3, 3>(3, 0)*dtR.transpose();
cov.block<3, 3>(3, 6) += -dtR * (pcov.block<3, 3>(0, 6) + dt*pcov.block<3, 3>(0, 9))
+ dt*pcov.block<3, 3>(3, 9);
cov.block<3, 3>(3, 9) += -dtR*( -pcov.block<3, 3>(0, 3)*dtQ.transpose() + pcov.block<3, 3>(0, 9))
- pcov.block<3, 3>(3, 3)*dtQ.transpose();
cov.block<3, 3>(6, 6) += dt*pcov.block<3, 3>(6, 9) + dt*dt*pcov.block<3, 3>(9, 9)
+ dt*pcov.block<3, 3>(9, 6);
cov.block<3, 3>(6, 9) += -pcov.block<3, 3>(6, 3)*dtQ.transpose() + dt*pcov.block<3, 3>(9, 9)
- dt*pcov.block<3, 3>(9, 3)*dtQ.transpose();
cov.block<3, 3>(9, 9) += dtQ*pcov.block<3, 3>(3, 3)*dtQ.transpose()
- dtQ*pcov.block<3, 3>(3, 9) - pcov.block<3, 3>(9, 3)*dtQ.transpose();
// Update symmetric cross-covariance terms
cov.block<3, 3>(3, 0) = cov.block<3, 3>(0, 3).transpose();
cov.block<3, 3>(6, 0) = cov.block<3, 3>(0, 6).transpose();
cov.block<3, 3>(6, 3) = cov.block<3, 3>(3, 6).transpose();
cov.block<3, 3>(9, 0) = cov.block<3, 3>(0, 9).transpose();
cov.block<3, 3>(9, 3) = cov.block<3, 3>(3, 9).transpose();
cov.block<3, 3>(9, 6) = cov.block<3, 3>(6, 9).transpose();
// Add state transition noise
cov.block<3, 3>(0, 0) += gyro_stability_noise.asDiagonal() * dt;
cov.block<3, 3>(3, 3) += gyro_white_noise.asDiagonal() * dt;
cov.block<3, 3>(6, 6) += accel_white_noise.asDiagonal() * 0.5*dt*dt;
cov.block<3, 3>(9, 9) += accel_white_noise.asDiagonal() * dt;
Quaterniond orientation = exp<double>((gyro_meas - avg_state.gyro_bias) * dt)
* avg_state.orientation;
// The contribution of the acceleration due to gravity on the accelerometer.
// Note that in NED, the z axis is down, and the accelerometer observes
// an extra acceleration due to gravity in the "up" direction.
Vector3d accel_gravity = -Vector3d::UnitZ() * 9.81;
Vector3d accel = accel_body - accel_gravity;
Vector3d position = avg_state.position + avg_state.velocity * dt + 0.5*accel*dt*dt;
Vector3d velocity = avg_state.velocity + accel*dt;
avg_state.position = position;
avg_state.velocity = velocity;
// Note: Renormalization occurs during all measurement updates.
avg_state.orientation = orientation;
assert(invariants_met());
#ifdef TIME_OPS
double time = clock.stop()*1e6;
std::cout << "unscented predict time: " << time << "\n";
#endif
}
bool Homography::
decompose()
{
decompositions.clear();
JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);
Vector3d singular_values = svd.singularValues();
double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
double d3 = fabs(singular_values[2]);
Matrix3d U = svd.matrixU();
Matrix3d V = svd.matrixV(); // VT^T
double s = U.determinant() * V.determinant();
double dPrime_PM = d2;
int nCase;
if(d1 != d2 && d2 != d3)
nCase = 1;
else if( d1 == d2 && d2 == d3)
nCase = 3;
else
nCase = 2;
if(nCase != 1)
{
printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
return false;
}
double x1_PM;
double x2;
double x3_PM;
// All below deals with the case = 1 case.
// Case 1 implies (d1 != d3)
{ // Eq. 12
x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
x2 = 0;
x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
};
double e1[4] = {1.0,-1.0, 1.0,-1.0};
double e3[4] = {1.0, 1.0,-1.0,-1.0};
Vector3d np;
HomographyDecomposition decomp;
// Case 1, d' > 0:
decomp.d = s * dPrime_PM;
for(size_t signs=0; signs<4; signs++)
{
// Eq 13
decomp.R = Matrix3d::Identity();
double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
decomp.R(0,0) = dCosTheta;
decomp.R(0,2) = -dSinTheta;
decomp.R(2,0) = dSinTheta;
decomp.R(2,2) = dCosTheta;
// Eq 14
decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions.push_back(decomp);
}
// Case 1, d' < 0:
decomp.d = s * -dPrime_PM;
for(size_t signs=0; signs<4; signs++)
{
// Eq 15
decomp.R = -1 * Matrix3d::Identity();
double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
decomp.R(0,0) = dCosPhi;
decomp.R(0,2) = dSinPhi;
decomp.R(2,0) = dSinPhi;
decomp.R(2,2) = -dCosPhi;
// Eq 16
decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions.push_back(decomp);
}
// Save rotation and translation of the decomposition
for(unsigned int i=0; i<decompositions.size(); i++)
{
Matrix3d R = s * U * decompositions[i].R * V.transpose();
Vector3d t = U * decompositions[i].t;
decompositions[i].T = Sophus::SE3(R, t);
}
return true;
}
void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
using namespace Eigen;
Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
Matrix3d EEt = E * E.transpose();
Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
Vector3d b1,b2;
#if 1
//Method 1
Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
Vector3d bbt_diag = bbt.diagonal();
if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
b1 = bbt.row(0) / sqrt(bbt_diag(0));
b2 = -b1;
} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
b1 = bbt.row(1) / sqrt(bbt_diag(1));
b2 = -b1;
} else {
b1 = bbt.row(2) / sqrt(bbt_diag(2));
b2 = -b1;
}
#else
//Method 2
if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else {
b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
}
#endif
//Horn90 (19)
Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
cofactors.transposeInPlace();
//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
Matrix3d B1; B1 << 0,-b1(2),b1(1),
b1(2),0,-b1(0),
-b1(1),b1(0),0;
Matrix3d B2; B2 << 0,-b2(2),b2(1),
b2(2),0,-b2(0),
-b2(1),b2(0),0;
Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);
//Horn90 (24)
R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
Map<Vector3d> t1(_t1),t2(_t2);
t1 = b1; t2 = b2;
cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}
