KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mfGridElementWidthInv(F.mfGridElementWidthInv),
mfGridElementHeightInv(F.mfGridElementHeightInv), mnTrackReferenceForFrame(0),mnBALocalForKF(0), mnBAFixedForKF(0),
mnLoopQuery(0), mnRelocQuery(0),fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), mBowVec(F.mBowVec),
im(F.im), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX), mnMaxY(F.mnMaxY), mK(F.mK),
mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn), mDescriptors(F.mDescriptors.clone()),
mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB), mpORBvocabulary(F.mpORBvocabulary), mFeatVec(F.mFeatVec),
mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false),
mnScaleLevels(F.mnScaleLevels), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
mvInvLevelSigma2(F.mvInvLevelSigma2), mpMap(pMap)
{
mnId=nNextId++;
mnGridCols=FRAME_GRID_COLS;
mnGridRows=FRAME_GRID_ROWS;
mGrid.resize(mnGridCols);
for(int i=0; i<mnGridCols;i++)
{
mGrid[i].resize(mnGridRows);
for(int j=0; j<mnGridRows; j++)
mGrid[i][j] = F.mGrid[i][j];
}
SetPose(F.mTcw);
}
Apriltag DetectorNode::DetectionToApriltagMsg(
const AprilTags::TagDetection &detection) {
Apriltag tag;
// Gather basic information
tag.id = detection.id;
tag.family = tag_family_;
tag.hamming_distance = detection.hammingDistance;
tag.center.x = detection.cxy.first;
tag.center.y = detection.cxy.second;
tag.size = tag_size_;
std::for_each(begin(detection.p), end(detection.p),
[&](const AprilTags::Pointf &corner) {
geometry_msgs::Point point;
point.x = corner.first;
point.y = corner.second;
tag.corners.push_back(point);
});
if (!cam_calibrated_) return tag;
// Get rotation and translation of tag in camera frame, only if we have cinfo!
Eigen::Quaterniond c_Q_b;
Eigen::Vector3d c_T_b;
/// @todo: Need to decide whether to undistort points here!
detection.getRelativeQT(tag_size_, model_.fullIntrinsicMatrix(),
model_.distortionCoeffs(), c_Q_b, c_T_b);
SetPose(&tag.pose, c_Q_b, c_T_b);
return tag;
}
void Viewer3dParam::Read( FILE *fp )
{
float tx, ty, tz, qw, qx, qy, qz;
if ( fscanf(fp, "%f%f%f%f%f%f%f", &tx, &ty, &tz, &qw, &qx, &qy, &qz) ==7 ) {
SetPose( Quaternion( qw, Vec3f(qx,qy,qz)), Vec3f(tx,ty,tz));
}
}
FeatureTracker::FeatureTracker(TooN::Vector<3,double> startPos):Visualizer()
{
srand(time(NULL));
SetPose(startPos);
// for(int i=0; i< 1000; i++)
// {
// Feature f;
// f.pos[0] = RAND(-5,5);
// f.pos[1] = RAND(-5,5);
// f.pos[2] = 0.01 + World::Instance()->GetHeight(f.pos[0], f.pos[1]);
// features.push_back(f);
// }
// vector<Entity*> ev;
// for(int i=0;i<10; i++)
// {
// double ww = 5;
// double wl = 5;
// Vector<3> p = makeVector(pow(-1,i)*i,i,4);//*/makeVector(RAND(-ww/2,ww/2), RAND(-wl/2,wl/2), 4);
// Entity * en = new Entity();
// en->pos = p;
// if(i==0)
// en->start = true;
// else
// en->start = false;
// ev.push_back(en);
// }
// Vector<3> p = makeVector(101,1,4);//*/makeVector(RAND(-ww/2,ww/2), RAND(-wl/2,wl/2), 4);
// Entity * en1 = new Entity();
// en1->pos = p;
// en1->start = false;
// ev.push_back(en1);
// p = makeVector(100,1,4);//*/makeVector(RAND(-ww/2,ww/2), RAND(-wl/2,wl/2), 4);
// Entity * en2 = new Entity();
// en2->pos = p;
// en2->start = false;
// ev.push_back(en2);
// shortestPath = tsp.GetShortestPath(ev);
}
void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel)
{
SetPose(pose);
SetVel(vel);
#if defined(ENABLE_RBD_PROFILER)
printf("RBD_Update_BEG\n");
#endif
UpdateJointSubspaceArr();
UpdateChildParentMatArr();
UpdateSpWorldTrans();
UpdateMassMat();
UpdateBiasForce();
#if defined(ENABLE_RBD_PROFILER)
printf("RBD_Update_End\n\n");
#endif
}
void CameraControllerBase::UpdateCameraPose( float dt )
{
if( !m_Active )
return;
float forward=0, right=0, up=0, spd;
// Matrix33 matRot;
if( (GetAsyncKeyState(VK_SHIFT) & 0x8000) )
spd = m_fTranslationSpeed * 2.0f;
else
spd = m_fTranslationSpeed * 1.0f;
int *kb = m_CameraControlCode;
if( IsKeyPressed(kb[CameraControl::Forward]) ) forward = spd * dt;
if( IsKeyPressed(kb[CameraControl::Backward]) ) forward = -spd * dt;
if( IsKeyPressed(kb[CameraControl::Right]) ) right = spd * dt;
if( IsKeyPressed(kb[CameraControl::Left]) ) right = -spd * dt;
if( IsKeyPressed(kb[CameraControl::Up]) ) up = spd * dt * 3.0f;
if( IsKeyPressed(kb[CameraControl::Down]) ) up = -spd * dt * 3.0f;
/*
// if( GetAsyncKeyState('X') & 0x8000 ) m_fYaw += 3.141592f * dt;
// if( GetAsyncKeyState('Z') & 0x8000 ) m_fYaw -= 3.141592f * dt;
// if( GetAsyncKeyState('Q') & 0x8000 ) m_fPitch += 3.141592f * dt;
// if( GetAsyncKeyState('A') & 0x8000 ) m_fPitch -= 3.141592f * dt;
// if( GetAsyncKeyState('Q') & 0x8000 ) m_vCameraPosition.y += 2.0f * dt;
// if( GetAsyncKeyState('A') & 0x8000 ) m_vCameraPosition.y -= 2.0f * dt;
*/
Matrix34 pose = GetPose();
pose.matOrient = Matrix33RotationY(m_fYaw) * Matrix33RotationX(m_fPitch);
pose.vPosition
+= pose.matOrient.GetColumn(2) * forward
+ pose.matOrient.GetColumn(0) * right
+ pose.matOrient.GetColumn(1) * up;
SetPose( pose );
// m_Pose.vPosition += m_vRight * m_fMouseMoveRight + m_vUp * m_fMouseMoveUp;
// m_fMouseMoveRight = 0; m_fMouseMoveUp = 0;
}
void FeatureTracker::MoveSensorTo(Vector<3,double> pos)
{
SetPose(pos);
}
void FeatureTracker::MoveSensor(TooN::Vector<3, double> dPos)
{
SetPose(pose + dPos);
}
RagDoll::RagDoll(char fileName[], D3DXMATRIX &world) : SkinnedMesh() {
SkinnedMesh::Load(fileName);
SetPose(world);
//InitRagdoll();
}
