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(); }