Repository: frobelbest/GSLAM Branch: master Commit: dcddfaf90644 Files: 71 Total size: 748.8 KB Directory structure: gitextract_116388qb/ ├── GSLAM/ │ ├── Drawer copy.h │ ├── Drawer.cpp │ ├── Drawer.h │ ├── Estimation.cpp │ ├── Estimation.h │ ├── Frame.cpp │ ├── Frame.h │ ├── Geometry.cpp │ ├── Geometry.h │ ├── GlobalReconstruction.cpp │ ├── GlobalReconstruction.h │ ├── GlobalRotationEstimation.cpp │ ├── GlobalScaleEstimation.cpp │ ├── GlobalTranslationEstimation.cpp │ ├── Homography.hpp │ ├── IMU.hpp │ ├── ImageGrids.hpp │ ├── KLT.h │ ├── KLTUtil.cpp │ ├── KLTUtil.h │ ├── KeyFrame.cpp │ ├── KeyFrame.h │ ├── KeyFrameConnection.cpp │ ├── KeyFrameConnection.h │ ├── KeyFrameDatabase.cpp │ ├── KeyFrameDatabase.h │ ├── L1Solver.h │ ├── LK.hpp │ ├── MapPoint.cpp │ ├── MapPoint.h │ ├── ORBVocabulary.h │ ├── ORBextractor.cc │ ├── ORBextractor.h │ ├── ORBmatcher.cc │ ├── ORBmatcher.h │ ├── RelativeMotion.hpp │ ├── Settings.h │ ├── System.cpp │ ├── System.h │ ├── TrackUtil.cpp │ ├── TrackUtil.h │ ├── Transform.cpp │ ├── Transform.h │ ├── convolve.cpp │ ├── convolve.h │ ├── error.cpp │ ├── error.h │ ├── experiment_helpers.hpp │ ├── las2.cpp │ ├── main.cpp │ ├── pyramid.cpp │ ├── pyramid.h │ ├── random_generators.hpp │ ├── selectGoodFeatures.hpp │ ├── selectGoodFeatures2.hpp │ ├── svdlib.cpp │ ├── svdlib.hpp │ ├── svdutil.cpp │ ├── svdutil.hpp │ └── time_measurement.hpp ├── GSLAM.xcodeproj/ │ ├── project.pbxproj │ ├── project.xcworkspace/ │ │ ├── contents.xcworkspacedata │ │ └── xcuserdata/ │ │ ├── chaos.xcuserdatad/ │ │ │ └── UserInterfaceState.xcuserstate │ │ └── ctang.xcuserdatad/ │ │ └── UserInterfaceState.xcuserstate │ └── xcuserdata/ │ ├── chaos.xcuserdatad/ │ │ ├── xcdebugger/ │ │ │ └── Breakpoints_v2.xcbkptlist │ │ └── xcschemes/ │ │ ├── GSLAM.xcscheme │ │ └── xcschememanagement.plist │ └── ctang.xcuserdatad/ │ ├── xcdebugger/ │ │ └── Breakpoints_v2.xcbkptlist │ └── xcschemes/ │ ├── GSLAM.xcscheme │ └── xcschememanagement.plist └── README.md ================================================ FILE CONTENTS ================================================ ================================================ FILE: GSLAM/Drawer copy.h ================================================ // // Drawer.h // GSLAM // // Created by ctang on 11/19/16. // Copyright © 2016 ctang. All rights reserved. // #ifndef DRAWER_H #define DRAWER_H #include "pangolin/pangolin.h" namespace GSLAM { class KeyFrame; class Drawer{ public: float mCameraSize; float mCameraLineWidth; float mGraphLineWidth; float mPointSize; int mFrameIndex; int mKeyFrameIndex; std::vector keyFrames; void drawCurrentCamera(pangolin::OpenGlMatrix &Twc); void getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &Twc); void drawKeyFrames(); void drawPoints(); }; } #include "KeyFrame.h" namespace GSLAM{ void Drawer::drawCurrentCamera(pangolin::OpenGlMatrix &Twc) { const float &w = mCameraSize; const float h = w*0.75; const float z = w*0.6; glPushMatrix(); glMultMatrixd(Twc.m); glLineWidth(mCameraLineWidth); glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0); glBegin(GL_LINES); glVertex3f(0,0,0); glVertex3f(w,h,z); glVertex3f(0,0,0); glVertex3f(w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(w,-h,z); glVertex3f(-w,h,z); glVertex3f(-w,-h,z); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(-w,-h,z); glVertex3f(w,-h,z); glEnd(); glPopMatrix(); } void Drawer::getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M){ if(mFrameIndex>=keyFrames[mKeyFrameIndex]->mvLocalFrames.back().frameId){ mKeyFrameIndex++; } printf("%d %d\n",mFrameIndex,keyFrames[mKeyFrameIndex]->frameId); Transform pose; Transform pose1; Transform pose2 if(mKeyFrameIndex<1){ pose1=keyFrames[mKeyFrameIndex]->pose; pose2=keyFrames[mKeyFrameIndex]->pose; }else{ pose1.rotation=keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.rotation *keyFrames[mKeyFrameIndex-1]->pose.rotation; Eigen::Vector3d translation; translation =keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.translation /keyFrames[mKeyFrameIndex-1]->scale; translation =keyFrames[mKeyFrameIndex-1]->pose.rotation.transpose()*translation; pose.translation+=translation; Transform pose2; pose2. } if (mFrameIndex==keyFrames[mKeyFrameIndex]->frameId) { }else if ((mFrameIndex-keyFrames[mKeyFrameIndex]->frameId)mvCloseFrames.size()) { /*int LocalIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId; pose.rotation=keyFrames[mKeyFrameIndex]->mvCloseFrames[LocalIndex].pose.rotation *keyFrames[mKeyFrameIndex]->pose.rotation; Eigen::Vector3d translation; translation =keyFrames[mKeyFrameIndex]->mvCloseFrames[LocalIndex].pose.translation /keyFrames[mKeyFrameIndex]->scale; translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation; pose.translation+=translation;*/ }else{ int localIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId-keyFrames[mKeyFrameIndex]->mvCloseFrames.size(); pose.rotation=keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.rotation *keyFrames[mKeyFrameIndex]->pose.rotation; Eigen::Vector3d translation; translation =keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.translation /keyFrames[mKeyFrameIndex]->scale; translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation; pose.translation+=translation; } pose.rotation.transposeInPlace(); M.m[0] = pose.rotation(0,0); M.m[1] = pose.rotation(1,0); M.m[2] = pose.rotation(2,0); M.m[3] = 0.0; M.m[4] = pose.rotation(0,1); M.m[5] = pose.rotation(1,1); M.m[6] = pose.rotation(2,1); M.m[7] = 0.0; M.m[8] = pose.rotation(0,2); M.m[9] = pose.rotation(1,2); M.m[10] = pose.rotation(2,2); M.m[11] = 0.0; M.m[12] = pose.translation(0); M.m[13] = pose.translation(1); M.m[14] = pose.translation(2); M.m[15] = 1.0; mFrameIndex++; } void Drawer::drawPoints(){ glPointSize(4); glBegin(GL_POINTS); for(int k=0;kmvLocalMapPoints.size();i++) { if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){ Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition(); point3D/=keyFrames[k]->scale; point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation; uchar* color=keyFrames[k]->mvLocalMapPoints[i].color; glColor3f(color[2]*(1.0/255.0),color[1]*(1.0/255.0),color[0]*(1.0/255.0)); glVertex3d(point3D(0),point3D(1),point3D(2)); } } } glEnd(); } void Drawer::drawKeyFrames(){ const float &w = mCameraSize; const float h = w*0.75; const float z = w*0.6; for(size_t i=0; i<=mKeyFrameIndex; i++){ KeyFrame* pKF = keyFrames[i]; Transform pose=pKF->pose; pose.rotation.transposeInPlace(); pangolin::OpenGlMatrix M; M.m[0] = pose.rotation(0,0); M.m[1] = pose.rotation(1,0); M.m[2] = pose.rotation(2,0); M.m[3] = 0.0; M.m[4] = pose.rotation(0,1); M.m[5] = pose.rotation(1,1); M.m[6] = pose.rotation(2,1); M.m[7] = 0.0; M.m[8] = pose.rotation(0,2); M.m[9] = pose.rotation(1,2); M.m[10] = pose.rotation(2,2); M.m[11] = 0.0; M.m[12] = pose.translation(0); M.m[13] = pose.translation(1); M.m[14] = pose.translation(2); M.m[15] = 1.0; glPushMatrix(); glMultMatrixd(M.m); glLineWidth(mCameraLineWidth*1.5); glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0); glBegin(GL_LINES); glVertex3f(0,0,0); glVertex3f(w,h,z); glVertex3f(0,0,0); glVertex3f(w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(w,-h,z); glVertex3f(-w,h,z); glVertex3f(-w,-h,z); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(-w,-h,z); glVertex3f(w,-h,z); glEnd(); glPopMatrix(); } glLineWidth(mGraphLineWidth); glBegin(GL_LINES); for(size_t k=0;k<=mKeyFrameIndex;k++) { // Covisibility Graph Eigen::Vector3d center=keyFrames[k]->pose.translation; for(map::iterator mit=keyFrames[k]->mConnectedKeyFramePoses.begin(), mend=keyFrames[k]->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ if(mit->first->mnIdmnId){ if(mit->first->nextKeyFramePtr==keyFrames[k]&&mit->first==keyFrames[k]->prevKeyFramePtr){ glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0); Eigen::Vector3d center2=mit->first->pose.translation; glVertex3d(center(0),center(1),center(2)); glVertex3d(center2(0),center2(1),center2(2)); }else if(mit->second.scale==-1.0){ glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0); Eigen::Vector3d center2=mit->first->pose.translation; glVertex3d(center(0),center(1),center(2)); glVertex3d(center2(0),center2(1),center2(2)); }else{ glColor3f(237.0f/255.0,177.0f/255.0,32.0f/255.0); Eigen::Vector3d center2=mit->first->pose.translation; glVertex3d(center(0),center(1),center(2)); glVertex3d(center2(0),center2(1),center2(2)); } } } } glEnd(); } } #endif ================================================ FILE: GSLAM/Drawer.cpp ================================================ // // Drawer.cpp // GSLAM // // Created by ctang on 11/19/16. // Copyright © 2016 ctang. All rights reserved. // ================================================ FILE: GSLAM/Drawer.h ================================================ // // Drawer.h // GSLAM // // Created by ctang on 11/19/16. // Copyright © 2016 ctang. All rights reserved. // #ifndef DRAWER_H #define DRAWER_H #include "pangolin/pangolin.h" #include "ceres/rotation.h" #include #include namespace GSLAM { class KeyFrame; class Drawer{ public: float mCameraSize; float mCameraLineWidth; float mGraphLineWidth; float mPointSize; int mFrameIndex; int mKeyFrameIndex; std::vector keyFrames; void drawCurrentCamera(pangolin::OpenGlMatrix &Twc); void drawCurrentTrajectory(pangolin::OpenGlMatrix &Twc); void getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &Twc); void drawKeyFrames(); void drawPoints(); std::vector preTwc; }; } #include "KeyFrame.h" namespace GSLAM{ void Drawer::drawCurrentTrajectory(pangolin::OpenGlMatrix &Twc) { const float &w = mCameraSize; const float h = w*0.75; const float z = w*0.6; glPushMatrix(); glMultMatrixd(Twc.m); glLineWidth(mCameraLineWidth); glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0); glBegin(GL_LINES); glVertex3f(0,0,0); glVertex3f(w,h,z); glVertex3f(0,0,0); glVertex3f(w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(w,-h,z); glVertex3f(-w,h,z); glVertex3f(-w,-h,z); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(-w,-h,z); glVertex3f(w,-h,z); glEnd(); glPopMatrix(); preTwc.push_back(Twc); glPointSize(4); glBegin(GL_POINTS); for(int i=0;i=keyFrames[mKeyFrameIndex]->mvLocalFrames.back().frameId){ preTwc.clear(); mKeyFrameIndex++; } //printf("%d %d\n",mFrameIndex,keyFrames[mKeyFrameIndex]->frameId); Transform pose; Transform pose1; Transform pose2; if(mKeyFrameIndex<1){ pose1=keyFrames[mKeyFrameIndex]->pose; pose2=keyFrames[mKeyFrameIndex]->pose; }else{ pose1.rotation=keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.rotation *keyFrames[mKeyFrameIndex-1]->pose.rotation; Eigen::Vector3d translation; translation =keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.translation /keyFrames[mKeyFrameIndex-1]->scale; translation =keyFrames[mKeyFrameIndex-1]->pose.rotation.transpose()*translation; pose1.translation=keyFrames[mKeyFrameIndex-1]->pose.translation+translation; pose2=keyFrames[mKeyFrameIndex]->pose; } if (mFrameIndex==keyFrames[mKeyFrameIndex]->frameId) { pose=pose1; }else if (mFrameIndexmvLocalFrames[0].frameId) { double distance1=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId; double distance2=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId-mFrameIndex; Eigen::Vector3d angle1,angle2,angle; ceres::RotationMatrixToAngleAxis(pose1.rotation.data(),angle1.data()); ceres::RotationMatrixToAngleAxis(pose2.rotation.data(),angle2.data()); angle=(distance2*angle1+distance1*angle2)/(distance1+distance2); ceres::AngleAxisToRotationMatrix(angle.data(),pose.rotation.data()); pose.translation=(distance2*pose1.translation+distance1*pose2.translation)/(distance1+distance2); pose1=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].pose; ceres::RotationMatrixToAngleAxis(pose1.rotation.data(),angle1.data()); angle1*=(distance1/(distance1+distance2)); ceres::AngleAxisToRotationMatrix(angle1.data(),pose1.rotation.data()); pose1.translation=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].pose.translation/keyFrames[mKeyFrameIndex]->scale; pose1.translation*=(distance1/(distance1+distance2)); pose.rotation=pose1.rotation*pose.rotation; pose.translation+=pose.rotation.transpose()*pose1.translation; }else{ int localIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId; pose.rotation=keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.rotation *keyFrames[mKeyFrameIndex]->pose.rotation; Eigen::Vector3d translation; translation =keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.translation /keyFrames[mKeyFrameIndex]->scale; translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation; pose.translation=pose2.translation+translation; } pose.rotation.transposeInPlace(); M.m[0] = pose.rotation(0,0); M.m[1] = pose.rotation(1,0); M.m[2] = pose.rotation(2,0); M.m[3] = 0.0; M.m[4] = pose.rotation(0,1); M.m[5] = pose.rotation(1,1); M.m[6] = pose.rotation(2,1); M.m[7] = 0.0; M.m[8] = pose.rotation(0,2); M.m[9] = pose.rotation(1,2); M.m[10] = pose.rotation(2,2); M.m[11] = 0.0; M.m[12] = pose.translation(0); M.m[13] = pose.translation(1); M.m[14] = pose.translation(2); M.m[15] = 1.0; mFrameIndex++; } void Drawer::drawPoints(){ glPointSize(4); glBegin(GL_POINTS); for(int k=0;kmvLocalMapPoints.size();i++) { if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){ Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition(); point3D/=keyFrames[k]->scale; point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation; uchar* color=keyFrames[k]->mvLocalMapPoints[i].color; glColor3f(color[2]*(1.0/255.0),color[1]*(1.0/255.0),color[0]*(1.0/255.0)); glVertex3d(point3D(0),point3D(1),point3D(2)); } } } glEnd(); } void Drawer::drawKeyFrames(){ const float &w = mCameraSize; const float h = w*0.75; const float z = w*0.6; std::vector centers(0); for(size_t i=0; i<=mKeyFrameIndex; i++){ KeyFrame* pKF = keyFrames[i]; Transform pose; if(i==0){ pose=pKF->pose; }else{ pose.rotation=keyFrames[i-1]->mvLocalFrames.back().pose.rotation *keyFrames[i-1]->pose.rotation; Eigen::Vector3d translation; translation =keyFrames[i-1]->mvLocalFrames.back().pose.translation/keyFrames[i-1]->scale; translation =keyFrames[i-1]->pose.rotation.transpose()*translation; pose.translation=keyFrames[i-1]->pose.translation+translation; } pose.rotation.transposeInPlace(); centers.push_back(pose.translation); pangolin::OpenGlMatrix M; M.m[0] = pose.rotation(0,0); M.m[1] = pose.rotation(1,0); M.m[2] = pose.rotation(2,0); M.m[3] = 0.0; M.m[4] = pose.rotation(0,1); M.m[5] = pose.rotation(1,1); M.m[6] = pose.rotation(2,1); M.m[7] = 0.0; M.m[8] = pose.rotation(0,2); M.m[9] = pose.rotation(1,2); M.m[10] = pose.rotation(2,2); M.m[11] = 0.0; M.m[12] = pose.translation(0); M.m[13] = pose.translation(1); M.m[14] = pose.translation(2); M.m[15] = 1.0; glPushMatrix(); glMultMatrixd(M.m); glLineWidth(mCameraLineWidth*1.5); glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0); glBegin(GL_LINES); glVertex3f(0,0,0); glVertex3f(w,h,z); glVertex3f(0,0,0); glVertex3f(w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(w,-h,z); glVertex3f(-w,h,z); glVertex3f(-w,-h,z); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(-w,-h,z); glVertex3f(w,-h,z); glEnd(); glPopMatrix(); } glLineWidth(mGraphLineWidth); glBegin(GL_LINES); //printf("show %d keyframes\n",centers.size()); std::vector > lines; for(size_t k=0;k<=mKeyFrameIndex;k++) { // Covisibility Graph Eigen::Vector3d center=centers[k];//=keyFrames[k]->pose.translation; for(map::iterator mit=keyFrames[k]->mConnectedKeyFramePoses.begin(), mend=keyFrames[k]->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ if(mit->first->mnIdmnId){ if(mit->first->nextKeyFramePtr==keyFrames[k]&&mit->first==keyFrames[k]->prevKeyFramePtr){ glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0); Eigen::Vector3d center2=centers[mit->first->mnId];//mit->first->pose.translation; glVertex3d(center(0),center(1),center(2)); glVertex3d(center2(0),center2(1),center2(2)); }else if(mit->second.scale==-1.0){ Eigen::Vector3d center2=centers[mit->first->mnId]; if(mit->first->frameId<70||keyFrames[k]->frameId<70){ lines.push_back(std::make_pair(center,center2)); continue; } glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0); glVertex3d(center(0),center(1),center(2)); glVertex3d(center2(0),center2(1),center2(2)); }else{ glColor3f(237.0f/255.0,177.0f/255.0,32.0f/255.0); Eigen::Vector3d center2=centers[mit->first->mnId];//mit->first->pose.translation; glVertex3d(center(0),center(1),center(2)); glVertex3d(center2(0),center2(1),center2(2)); } } } } glEnd(); glLineWidth(mGraphLineWidth); glLineStipple(8, 0xAAAA); glEnable(GL_LINE_STIPPLE); glBegin(GL_LINES); for(int i=0;i #include "Geometry.h" #include "RelativeMotion.hpp" #include "opencv2/calib3d/calib3d.hpp" bool getPVector(const Eigen::Vector3d &T12, const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, Eigen::Vector3d &pVector){ Eigen::Vector3d T=p1; Eigen::Vector3d r1=T12; Eigen::Vector3d r2=-p2; Eigen::Vector3d xij=r1.cross(r2); //analytic solution Eigen::Matrix3d A; for(int i=0;i<3;i++){ A(i,0)=r1(i); A(i,1)=-r2(i); A(i,2)=-xij(i); } double determinantA=A.determinant(); Eigen::Matrix3d D=A; D.col(0)=T; double determinant1=D.determinant(); A.col(1)=T; double determinant2=A.determinant(); double s_ik=determinant1/determinantA; double s_jk=determinant2/determinantA; if(s_ik<0||s_jk<0){ return false; } Eigen::Matrix3d R_jik=ComputeAxisRotation(T,r1); Eigen::Matrix3d R_ijk=ComputeAxisRotation(-T,r2); Eigen::Matrix3d PMatrix=(Eigen::Matrix3d::Identity()+s_ik*R_jik-s_jk*R_ijk); pVector=PMatrix*p1; pVector=0.5*pVector; return true; } void getScaleRatio(const Eigen::Vector3d &T12, const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, double &s1,double &s2){ Eigen::Vector3d T=p1; Eigen::Vector3d r1=T12; Eigen::Vector3d r2=-p2; Eigen::Vector3d xij=r1.cross(r2); //analytic solution Eigen::Matrix3d A; for(int i=0;i<3;i++){ A(i,0)=r1(i); A(i,1)=-r2(i); A(i,2)=-xij(i); } double determinantA=A.determinant(); Eigen::Matrix3d D=A; D.col(0)=T; double determinant1=D.determinant(); A.col(1)=T; double determinant2=A.determinant(); s1=determinant1/determinantA; s2=determinant2/determinantA; } Eigen::Vector3d estimateRelativeTranslationIRLS(const std::vector &pts1, const std::vector &pts2, std::vector &pVectors, std::vector &status){ int k = 0; int kmax = 100; int max_in_iter = 10; int in_iter = 0; double epslon = 1e-5; int num_point=pts1.size(); Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point); for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); Eigen::Vector3d translationNew=V.col(2); Eigen::VectorXd weightsNew=(translationNew.transpose()*allNorms); weights=weightsNew.cwiseAbs(); double costNew=weights.sum(); double delt = std::max(std::abs(costVal-costNew),1-translationNew.dot(translation)); if (delt <= epslon){ in_iter++; } else{ in_iter=0; } costVal=costNew; translation=translationNew; k++; break; } pVectors.resize(num_point); status.resize(num_point); std::fill(status.begin(),status.end(),true); std::vector s1(num_point),s2(num_point); for (int i=0;i0; } bool isPositive=positiveCount>overallCount/2; double factor=isPositive?1.0:-1.0; translation*=factor; for (int i=0;i0)==isPositive) { double s_1=factor*s1[i]; double s_2=factor*s2[i]; Eigen::Matrix3d R_jik=ComputeAxisRotation(pts1[i],translation); Eigen::Matrix3d R_ijk=ComputeAxisRotation(-pts1[i],-pts2[i]); Eigen::Matrix3d PMatrix=(Eigen::Matrix3d::Identity()+s_1*R_jik-s_2*R_ijk); pVectors[i]=0.5*PMatrix*pts1[i]; }else{ status[i]=false; } } } return translation; } Eigen::Vector3d estimateRelativeTranslation(const std::vector &pts1, const std::vector &pts2, std::vector &pVectors, std::vector &status){ int num_point=pts1.size(); std::vector norms(num_point); pVectors.resize(num_point); status.resize(num_point); Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point); Eigen::Vector3d preResult,curResult; for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); curResult=V.col(2); int positiveCount=0; std::vector flags(num_point); for(int i=0;i0); positiveCount+=flags[i]; } int flag=1; if((double)positiveCount<0.5*(double)num_point){ curResult*=-1; flag=0; } for(int i=0;i &pts1, const std::vector &pts2, std::vector &status, const double inlierPercentage=0.8, const double inlierThreshold=std::sin((3.0/180.0)*CV_PI)){ int num_point=pts1.size(); std::vector residuals(num_point); std::vector norms(num_point); status.resize(num_point); std::vector pVectors(num_point); Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point); int numInliers=0; double preError=1.0,curError=1.0; Eigen::Vector3d preResult,curResult; for(int iter=0;iter<10;iter++){ if(iter==0){ for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); curResult=V.col(2); Eigen::VectorXd errors=curResult.transpose()*allNorms; for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); curResult=V.col(2); Eigen::VectorXd errors=curResult.transpose()*allNorms; for(int i=0;i0); positiveCount+=residuals[i].flag; } int flag=1; if((double)positiveCount<0.5*(double)numInliers){ preResult*=-1; flag=0; } for(int i=0;i &pts1, const std::vector &pts2, std::vector &pVectors, std::vector &status, std::vector &errors, double &medError, const double inlierPercentage=0.8, const double inlierThreshold=std::sin((3.0/180.0)*CV_PI)){ int num_point=pts1.size(); std::vector residuals(num_point); std::vector norms(num_point); pVectors.resize(num_point); status.resize(num_point); errors.resize(num_point); Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point); int numInliers=0; double preError=1.0,curError=1.0; Eigen::Vector3d preResult,curResult; for(int iter=0;iter<10;iter++){ if(iter==0){ for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); curResult=V.col(2); Eigen::VectorXd errors=curResult.transpose()*allNorms; for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); curResult=V.col(2); Eigen::VectorXd errors=curResult.transpose()*allNorms; for(int i=0;i0); positiveCount+=residuals[i].flag; } int flag=1; if((double)positiveCount<0.5*(double)numInliers){ preResult*=-1; flag=0; } for(int i=0;i &pts1, std::vector &pts2, Eigen::Matrix3d &rotation, Eigen::Vector3d &translation, std::vector &pVectors, std::vector &status){ estimateRotationTranslation(0.005, pts1, pts2, rotation, translation); //estimateRotationTranslation2(0.005,pts1,pts2,rotation,translation); translation=rotation.transpose() * translation; translation.normalize(); for (int i=0;i flags(num_point); for(int i=0;i0); positiveCount+=flags[i]; } int flag=1; if((double)positiveCount<0.5*(double)num_point){ translation*=-1; flag=0; } status.resize(num_point); std::fill(status.begin(),status.end(),false); pVectors.resize(num_point); for(int i=0;i bool operator()(const T* const camera, const T* const depth, T* residuals) const { T p[3]; T point[3]={((T)key_x)/depth[0],((T)key_y)/depth[0],1.0/depth[0]}; ceres::AngleAxisRotatePoint(camera,point,p); p[0]+=camera[3]; p[1]+=camera[4]; p[2]+=camera[5]; T predicted_x=p[0]/p[2]; T predicted_y=p[1]/p[2]; residuals[0]=predicted_x-(T)tracked_x; residuals[1]=predicted_y-(T)tracked_y; residuals[0]=residuals[0]*weight; residuals[1]=residuals[1]*weight; return true; } static ceres::CostFunction* Create(const double key_x, const double key_y, const double tracked_x, const double tracked_y, const double weight=1.0) { return (new ceres::AutoDiffCostFunction(new SnavelyReprojectionError(key_x,key_y,tracked_x,tracked_y,weight))); }; double tracked_x; double tracked_y; double key_x; double key_y; double weight; }; struct SnavelyReprojectionErrorSimple { SnavelyReprojectionErrorSimple( const double _key_x,const double _key_y, const double _tracked_x,const double _tracked_y): key_x(_key_x),key_y(_key_y), tracked_x(_tracked_x), tracked_y(_tracked_y){ } template bool operator()(const T* const camera, const T* const depth, T* residuals) const { T ax=key_x-camera[2]*key_y+camera[1]; T bx=camera[3]; T ay=key_y-camera[0]+camera[2]*key_x; T by=camera[4]; T c =-camera[1]*key_x+camera[0]*key_y+1.0; T d =camera[5]; T ex=tracked_x*c-ax; T ey=tracked_y*c-ay; if(depth[0]<0.01){ residuals[0]=ex; residuals[1]=ey; return true; } T fx=tracked_x*d-bx; T fy=tracked_y*d-by; residuals[0]=(ex+fx*depth[0])/(c+d*depth[0]); residuals[1]=(ey+fy*depth[0])/(c+d*depth[0]); return true; } static ceres::CostFunction* Create( const double key_x, const double key_y, const double tracked_x, const double tracked_y) { return (new ceres::AutoDiffCostFunction(new SnavelyReprojectionErrorSimple(key_x,key_y,tracked_x,tracked_y))); } double tracked_x; double tracked_y; double key_x; double key_y; }; void setSmallRotationMatrix(const double *w,Eigen::Matrix3d &rotation){ rotation(0,0)=rotation(1,1)=rotation(2,2)=1.0; rotation(0,1)=-w[2];rotation(0,2)=w[1]; rotation(1,0)=w[2];rotation(1,2)=-w[0]; rotation(2,0)=-w[1];rotation(2,1)=w[0]; } namespace GSLAM{ int RelativeOutlierRejection::relativeRansac(const KeyFrame *keyFrame,const KLT_FeatureList featureList){ assert(keyFrame->mvLocalMapPoints.size()==featureList->nFeatures); vector points1,points2; vector indices; points1.reserve(featureList->nFeatures); points2.reserve(featureList->nFeatures); indices.reserve(featureList->nFeatures); int medViewAngleCount=0,overallCount=0; Eigen::Matrix3d rotationTranspoed=rotation.transpose(); for (int i=0;inFeatures;i++) { if(featureList->feature[i]->val==KLT_TRACKED&&(*isValid)[i]){ overallCount++; Eigen::Vector3d p1=keyFrame->mvLocalMapPoints[i].norm; Eigen::Vector3d p2=rotationTranspoed*featureList->feature[i]->norm; p2.normalize(); double viewAngle=p1.dot(p2); if (viewAngle>=minViewAngle) { (*isValid)[i]=false; continue; } if (viewAngle status; cv::findFundamentalMat(points1,points2,cv::FM_RANSAC,theshold,prob,status); int inlierCount=status.size(); for (int i=0;i &pVectors){ std::vector viewAngles(keyFrame->mvLocalMapPoints.size()); std::fill(viewAngles.begin(),viewAngles.end(),-1.0); pVectors.resize(keyFrame->mvLocalMapPoints.size()); std::fill(pVectors.begin(),pVectors.end(),static_cast(NULL)); std::vector norms1,norms2; std::vector indices; indices.reserve(keyFrame->mvLocalMapPoints.size()); norms1.reserve(keyFrame->mvLocalMapPoints.size()); norms2.reserve(keyFrame->mvLocalMapPoints.size()); for (int i=0;imvLocalMapPoints.size();i++) { if (featureList->feature[i]->val==KLT_TRACKED&&(*isValid)[i]) { Eigen::Vector3d norm2=rotation.transpose()*featureList->feature[i]->norm; norm2.normalize(); norms1.push_back(keyFrame->mvLocalMapPoints[i].norm); norms2.push_back(norm2); indices.push_back(i); } } norms1.shrink_to_fit(); norms2.shrink_to_fit(); indices.shrink_to_fit(); std::vector status; std::vector tmpPVectors; translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status); //translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status); //Eigen::Matrix3d inputRotation; //Eigen::Vector3d inputTranslation; //this->rotation=Eigen::Matrix3d::Identity(); //this->translation=Eigen::Vector3d::Zero(); //estimateRelativeMotion(norms1,norms2,this->rotation,this->translation,tmpPVectors,status); /*Eigen::Matrix3d transposedRotation=rotation.transpose(); for (int i=0;i indices(0); for(int i=0;imvLocalMapPoints.size();i++){ keyFrame->mvLocalMapPoints[i].isFullTrack&=(!keyFrame->mvLocalMapPoints[i].vecs.empty()); if(keyFrame->mvLocalMapPoints[i].isFullTrack){ indices.push_back(i); } } int num_frame=keyFrame->mvLocalFrames.size(); int num_point=indices.size(); //printf("%d %d\n",num_frame,num_point);getchar(); DMat denseMatrix=svdNewDMat(num_point,3*num_frame); for(int p=0;p& pVectors=keyFrame->mvLocalMapPoints[ind].pVectors; for(int f=0;fvalue[p][3*f],&(*pVectors[f])(0),3*sizeof(double)); } } SMat sparseMatrix=svdConvertDtoS(denseMatrix); SVDRec svdResult=svdLAS2A(sparseMatrix,12); #ifdef DEBUG printf("singular %f %f\n",svdResult->S[0],svdResult->S[1]); #endif svdFreeDMat(denseMatrix); svdFreeSMat(sparseMatrix); int positiveCount=0; for(int p=0;pUt->value[0][p]>0); } if(positiveCountUt->value[0][p]*=positiveCount; int ind=indices[p]; if (svdResult->Ut->value[0][p]>0) { keyFrame->mvLocalMapPoints[ind].invdepth=svdResult->Ut->value[0][p]/keyFrame->mvLocalMapPoints[ind].norm(2); keyFrame->mvLocalMapPoints[ind].isEstimated=true; } } for(int f=0;fmvLocalFrames[f].pose.translation=Eigen::Vector3d(&svdResult->Vt->value[0][3*f]); keyFrame->mvLocalFrames[f].pose.translation*=positiveCount*svdResult->S[0]; } } void LocalFactorization::iterativeRefine(KeyFrame *keyFrame){ std::vector indices(0); for(int i=0;imvLocalMapPoints.size();i++){ keyFrame->mvLocalMapPoints[i].isFullTrack&=(!keyFrame->mvLocalMapPoints[i].vecs.empty()); if(keyFrame->mvLocalMapPoints[i].isFullTrack){ indices.push_back(i); } } std::vector vecs1(indices.size()); std::vector vecs2(indices.size()); /*for (int i=0;imvLocalMapPoints[indices[i]].vec; vecs0[i].x=vec(0); vecs0[i].y=vec(1); }*/ std::ofstream of("/Users/ctang/Desktop/debug/res.txt"); //double converge=0.002; for (int f=keyFrame->mvLocalFrames.size()-5;fmvLocalFrames.size()-4;f++) { for (int i=0;imvLocalMapPoints[indices[i]].getPosition(); Eigen::Vector3d vec1=keyFrame->mvLocalFrames[f].pose.rotation*(point3d-keyFrame->mvLocalFrames[f].pose.translation); vec1/=vec1(2); Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[indices[i]].vecs[f]; vecs1[i].x=vec1(0); vecs1[i].y=vec1(1); vecs2[i].x=vec2(0); vecs2[i].y=vec2(1); } std::vector statuts; cv::Mat homography=cv::findHomography(vecs1,vecs2,CV_RANSAC,0.005,statuts); std::vector rotation(1),translation(1),normal(1); cv::Mat calib=cv::Mat::eye(3,3,CV_64FC1); cv::decomposeHomographyMat(homography,calib, rotation,translation,normal); /*printf("%d\n",f); for (int i1=0;i1<3;i1++) { for (int i2=0;i2<3;i2++) { printf("%f ",rotation[0].at(i1,i2)); } printf("\n"); }*/ //printf("%d\n",rotation.size()); //getchar(); for (int i=0;i(0)=vecs1[i].x; vec1.at(1)=vecs1[i].y; vec1.at(2)=1.0; double diffx=vec1.at(0)-vecs2[i].x; double diffy=vec1.at(1)-vecs2[i].y; of<(2); diffx=_vec1.at(0)-vecs2[i].x; diffy=_vec1.at(1)-vecs2[i].y; of< indices; for (int p=0;pmvLocalMapPoints.size();p++) { if (keyFrame->mvLocalMapPoints[p].isEstimated) { indices.push_back(p); num_point++; } } int num_frame=keyFrame->mvLocalFrames.size(); std::vector pointNumberInFrames(num_frame,num_point); std::vector param_cam(6*num_frame),param_invDepth(num_point); ceres::Problem problem; printf("%d %d\n",num_point,num_frame); for(int p=0;pmvLocalMapPoints[index].invdepth; } for(int f=0;fmvLocalFrames[f].pose.rotation(0),¶m_cam[6*f]); Eigen::Vector3d translation=keyFrame->mvLocalFrames[f].pose.rotation*keyFrame->mvLocalFrames[f].pose.translation; param_cam[6*f+3]=-translation(0); param_cam[6*f+4]=-translation(1); param_cam[6*f+5]=-translation(2); } ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005); for(int p=0;pmvLocalMapPoints[index].vec; for(int f=0;fmvLocalMapPoints[index].vecs[f]==NULL){ continue; } Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[index].vecs[f]; ceres::CostFunction* cost_function =SnavelyReprojectionError::Create(vec1(0),vec1(1),vec2(0),vec2(1)); problem.AddResidualBlock(cost_function,loss_function,¶m_cam[6*f],¶m_invDepth[p]); } } if (cameraFixed) { for(int f=0;fmvLocalFrames[f].pose.rotation; ceres::AngleAxisToRotationMatrix(¶m_cam[6*f],&keyFrame->mvLocalFrames[f].pose.rotation(0)); //rotation=rotation.transpose()*keyFrame->mvLocalFrames[f].pose.rotation; //std::cout<mvLocalFrames[f].pose.translation=-Eigen::Vector3d(¶m_cam[6*f+3]); keyFrame->mvLocalFrames[f].pose.translation=keyFrame->mvLocalFrames[f].pose.rotation.transpose() *keyFrame->mvLocalFrames[f].pose.translation; keyFrame->mvLocalFrames[f].pose.setEssentialMatrix(); } for(int p=0;pmvLocalMapPoints[index].invdepth=param_invDepth[p]; keyFrame->mvLocalMapPoints[index].updateNormalAndDepth(); } for(int p=0;pmvLocalMapPoints[index].getPosition(); if(keyFrame->mvLocalMapPoints[index].errors.empty()){ keyFrame->mvLocalMapPoints[index].errors.resize(num_frame); } for (int f=0;fmvLocalMapPoints[index].vecs[f]==NULL){ continue; } Eigen::Vector3d projected=keyFrame->mvLocalFrames[f].pose.rotation *(point3D-keyFrame->mvLocalFrames[f].pose.translation); projected/=projected(2); projected-=(*keyFrame->mvLocalMapPoints[index].vecs[f]); double error=projected.norm(); keyFrame->mvLocalMapPoints[index].errors[f]=error; if (error>projErrorThres) { delete keyFrame->mvLocalMapPoints[index].vecs[f]; keyFrame->mvLocalMapPoints[index].vecs[f]=NULL; keyFrame->mvLocalMapPoints[index].measurementCount--; keyFrame->mvLocalFrames[f].measurementCount--; } } //assert(keyFrame->mvLocalMapPoints[index].measurementCount>0); if (keyFrame->mvLocalMapPoints[index].measurementCount==0||keyFrame->mvLocalMapPoints[index].invdepth<0) { keyFrame->mvLocalMapPoints[index].isEstimated=false; } } /*for (int i=0;imvLocalMapPoints.size();i++) { }*/ int measurementCountByPoint=0,measurementCountByFrame=0; for (int p=0;pmvLocalMapPoints.size();p++) { measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount; } for (int f=0;fmvLocalFrames.size();f++) { measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount; } printf("%d %d\n",measurementCountByPoint,measurementCountByFrame); assert(measurementCountByPoint==measurementCountByFrame); } void LocalBundleAdjustment::triangulate(KeyFrame* keyFrame){ int num_frame=keyFrame->mvLocalFrames.size(); for (int p=0;pmvLocalMapPoints.size();p++) { if(!keyFrame->mvLocalMapPoints[p].isEstimated&&keyFrame->mvLocalMapPoints[p].isValid()){ std::vector normalized; std::vector rotations; std::vector cameras; normalized.reserve(num_frame); rotations.reserve(num_frame); cameras.reserve(num_frame); normalized.push_back(keyFrame->mvLocalMapPoints[p].norm); rotations.push_back(Eigen::Matrix3d::Identity()); cameras.push_back(Eigen::Vector3d::Zero()); for (int f=0;fmvLocalMapPoints[p].vecs[f]==NULL) { continue; } Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[p].vecs[f]; Eigen::Vector3d norm2=keyFrame->mvLocalFrames[f].pose.rotation.transpose()*vec2; norm2.normalize(); double viewAngle=keyFrame->mvLocalMapPoints[p].norm.dot(norm2); if (viewAngle>viewAngleThres) { delete keyFrame->mvLocalMapPoints[p].vecs[f]; keyFrame->mvLocalMapPoints[p].vecs[f]=NULL; keyFrame->mvLocalMapPoints[p].measurementCount--; keyFrame->mvLocalFrames[f].measurementCount--; }else{ Eigen::Vector3d epiolarLine=keyFrame->mvLocalFrames[f].pose.E*keyFrame->mvLocalMapPoints[p].vec; double squareError=epiolarLine.dot(vec2); squareError*=squareError; squareError/=(epiolarLine(0)*epiolarLine(0)+epiolarLine(1)*epiolarLine(1)); if (squareError>4*projErrorThres*projErrorThres) { delete keyFrame->mvLocalMapPoints[p].vecs[f]; keyFrame->mvLocalMapPoints[p].vecs[f]=NULL; keyFrame->mvLocalMapPoints[p].measurementCount--; keyFrame->mvLocalFrames[f].measurementCount--; }else{ normalized.push_back(norm2); rotations.push_back(Eigen::Matrix3d::Identity()); cameras.push_back(keyFrame->mvLocalFrames[f].pose.translation); } } } normalized.shrink_to_fit(); rotations.shrink_to_fit(); cameras.shrink_to_fit(); if (cameras.size()>1) { Eigen::Vector3d point3D=multiviewTriangulationLinear(normalized,rotations,cameras); bool isOutlier=false; for(int i=1;i2*projErrorThres) { isOutlier=true; break; } } if (point3D(2)<0) { isOutlier=true; } if (!isOutlier) { keyFrame->mvLocalMapPoints[p].isEstimated=true; keyFrame->mvLocalMapPoints[p].invdepth=1.0/point3D(2); }else{ for (int f=0;fmvLocalMapPoints[p].vecs[f]!=NULL) { delete keyFrame->mvLocalMapPoints[p].vecs[f]; keyFrame->mvLocalMapPoints[p].vecs[f]=NULL; keyFrame->mvLocalFrames[f].measurementCount--; } } keyFrame->mvLocalMapPoints[p].measurementCount=0; } } } } //zheli int measurementCountByPoint=0,measurementCountByFrame=0; for (int p=0;pmvLocalMapPoints.size();p++) { measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount; } for (int f=0;fmvLocalFrames.size();f++) { measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount; } assert(measurementCountByPoint==measurementCountByFrame); int effectiveCount=0; for(int i=0;imvLocalMapPoints.size();i++){ effectiveCount+=keyFrame->mvLocalMapPoints[i].isValid()==true; } printf("%d\n",effectiveCount); } //refine points by projection error void LocalBundleAdjustment::refinePoints(KeyFrame *keyFrame){ int num_point=0; std::vector indices; for (int p=0;pmvLocalMapPoints.size();p++) { if (keyFrame->mvLocalMapPoints[p].isEstimated) { indices.push_back(p); num_point++; } } int num_frame=keyFrame->mvLocalFrames.size(); for(int p=0;pmvLocalMapPoints[index].getPosition(); if(keyFrame->mvLocalMapPoints[index].errors.empty()){ keyFrame->mvLocalMapPoints[index].errors.resize(num_frame); } for (int f=0;fmvLocalMapPoints[index].vecs[f]==NULL){ continue; } Eigen::Vector3d projected=keyFrame->mvLocalFrames[f].pose.rotation *(point3D-keyFrame->mvLocalFrames[f].pose.translation); projected/=projected(2); projected-=(*keyFrame->mvLocalMapPoints[index].vecs[f]); double error=projected.norm(); keyFrame->mvLocalMapPoints[index].errors[f]=error; if (error>projErrorThres) { delete keyFrame->mvLocalMapPoints[index].vecs[f]; keyFrame->mvLocalMapPoints[index].vecs[f]=NULL; keyFrame->mvLocalMapPoints[index].measurementCount--; keyFrame->mvLocalFrames[f].measurementCount--; } } if (keyFrame->mvLocalMapPoints[index].measurementCount==0 ||keyFrame->mvLocalMapPoints[index].invdepth<0) { keyFrame->mvLocalMapPoints[index].isEstimated=false; } } int measurementCountByPoint=0,measurementCountByFrame=0; for (int p=0;pmvLocalMapPoints.size();p++) { measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount; } for (int f=0;fmvLocalFrames.size();f++) { measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount; } printf("%d %d\n",measurementCountByPoint,measurementCountByFrame); assert(measurementCountByPoint==measurementCountByFrame); } void LocalBundleAdjustment::triangulate2(KeyFrame* keyFrame){ int num_frame=keyFrame->mvLocalFrames.size(); for (int p=0;pmvLocalMapPoints.size();p++) { if(!keyFrame->mvLocalMapPoints[p].isEstimated&&keyFrame->mvLocalMapPoints[p].isValid()){ std::vector normalized; std::vector rotations; std::vector cameras; normalized.reserve(num_frame); rotations.reserve(num_frame); cameras.reserve(num_frame); normalized.push_back(keyFrame->mvLocalMapPoints[p].norm); rotations.push_back(Eigen::Matrix3d::Identity()); cameras.push_back(Eigen::Vector3d::Zero()); for (int f=0;fmvLocalMapPoints[p].vecs[f]==NULL) { continue; } Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[p].vecs[f]; Eigen::Vector3d norm2=keyFrame->mvLocalFrames[f].pose.rotation.transpose()*vec2; norm2.normalize(); double viewAngle=keyFrame->mvLocalMapPoints[p].norm.dot(norm2); if (viewAngle>viewAngleThres) { delete keyFrame->mvLocalMapPoints[p].vecs[f]; keyFrame->mvLocalMapPoints[p].vecs[f]=NULL; keyFrame->mvLocalMapPoints[p].measurementCount--; keyFrame->mvLocalFrames[f].measurementCount--; }else{ Eigen::Vector3d epiolarLine=keyFrame->mvLocalFrames[f].pose.E *keyFrame->mvLocalMapPoints[p].vec; double squareError=epiolarLine.dot(vec2); squareError*=squareError; squareError/=(epiolarLine(0)*epiolarLine(0)+epiolarLine(1)*epiolarLine(1)); if (squareError>4*projErrorThres*projErrorThres) { delete keyFrame->mvLocalMapPoints[p].vecs[f]; keyFrame->mvLocalMapPoints[p].vecs[f]=NULL; keyFrame->mvLocalMapPoints[p].measurementCount--; keyFrame->mvLocalFrames[f].measurementCount--; }else{ normalized.push_back(norm2); rotations.push_back(Eigen::Matrix3d::Identity()); cameras.push_back(keyFrame->mvLocalFrames[f].pose.translation); } } } normalized.shrink_to_fit(); rotations.shrink_to_fit(); cameras.shrink_to_fit(); if (cameras.size()>1) { Eigen::Vector3d point3D=multiviewTriangulationLinear(normalized,rotations,cameras); //double depth=multiviewTriangulationDepth(normalized,rotations,cameras); //std::cout<2*projErrorThres) { isOutlier=true; break; } } if (point3D(2)<0) { isOutlier=true; } if (!isOutlier) { keyFrame->mvLocalMapPoints[p].isEstimated=true; keyFrame->mvLocalMapPoints[p].invdepth=1.0/point3D(2); }else{ for (int f=0;fmvLocalMapPoints[p].vecs[f]!=NULL) { delete keyFrame->mvLocalMapPoints[p].vecs[f]; keyFrame->mvLocalMapPoints[p].vecs[f]=NULL; keyFrame->mvLocalFrames[f].measurementCount--; } } keyFrame->mvLocalMapPoints[p].measurementCount=0; } } } } int measurementCountByPoint=0,measurementCountByFrame=0; for (int p=0;pmvLocalMapPoints.size();p++) { measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount; } for (int f=0;fmvLocalFrames.size();f++) { measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount; } assert(measurementCountByPoint==measurementCountByFrame); int effectiveCount=0; for(int i=0;imvLocalMapPoints.size();i++){ effectiveCount+=keyFrame->mvLocalMapPoints[i].isValid()==true; } printf("%d\n",effectiveCount); } /*void LocalBundleAdjustment::estimateFarFrames(KeyFrame* keyFrame){ for (int i=0;imvFarFrames.size();i++) { std::vector& closeMeasurements=keyFrame->mvFarMeasurements[i]; std::vector points3d; std::vector points2d; for (int m=0;mmvLocalMapPoints[m].measurementCount>0) { cv::Point3f point3d; cv::Point2f point2d; Eigen::Vector3d point3D=keyFrame->mvLocalMapPoints[m].getPosition(); point3d.x=point3D(0); point3d.y=point3D(1); point3d.z=point3D(2); Eigen::Vector3d normalized3d=*closeMeasurements[m]; point2d.x=normalized3d(0)/normalized3d(2); point2d.y=normalized3d(1)/normalized3d(2); points3d.push_back(point3d); points2d.push_back(point2d); //printf("3d %f %f %f %f %f\n",point3d.x,point3d.y,point3d.z,point2d.x,point2d.y); } } assert(points3d.size()>10); cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1); cv::Mat rVec,tVec,rMat; cv::solvePnPRansac(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec,false,1000,0.008,0.99); cv::Rodrigues(rVec,rMat); Transform pose; for(int r1=0;r1<3;r1++){ for(int r2=0;r2<3;r2++){ pose.rotation(r1,r2)=rMat.at(r1,r2); } } tVec=-rMat.t()*tVec; assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1); pose.translation(0)=tVec.at(0); pose.translation(1)=tVec.at(1); pose.translation(2)=tVec.at(2); double param_cam[6]; ceres::RotationMatrixToAngleAxis(&pose.rotation(0),¶m_cam[0]); Eigen::Vector3d translation=pose.rotation*pose.translation; param_cam[3]=-translation(0); param_cam[4]=-translation(1); param_cam[5]=-translation(2); vector param_invDepth(points3d.size()); ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005); int nInliers=0; for (int i=0;i(0)=points3d[i].x; point3dMat.at(1)=points3d[i].y; point3dMat.at(2)=points3d[i].z; cv::Mat proj=rMat*point3dMat+tVec; proj/=proj.at(2); double diffx=proj.at(0)-points2d[i].x; double diffy=proj.at(1)-points2d[i].y; double error=std::sqrt(diffx*diffx+diffy*diffy); param_invDepth[i]=1.0/point3dMat.at(2); point3dMat/=point3dMat.at(2); if (error<0.005) { ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(point3dMat.at(0), point3dMat.at(1), (double)points2d[i].x, (double)points2d[i].y); problem.AddResidualBlock(cost_function,loss_function,param_cam,¶m_invDepth[i]); problem.SetParameterBlockConstant(¶m_invDepth[i]); nInliers++; } } ceres::Solver::Options options; options.max_num_iterations=5; options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; options.minimizer_progress_to_stdout=false; ceres::Solver::Summary summary; ceres::Solve(options,&problem, &summary); ceres::AngleAxisToRotationMatrix(param_cam,&(keyFrame->mvFarFrames[i].pose.rotation(0))); keyFrame->mvFarFrames[i].pose.translation=-Eigen::Vector3d(¶m_cam[3]); keyFrame->mvFarFrames[i].pose.translation=keyFrame->mvFarFrames[i].pose.rotation.transpose() *keyFrame->mvFarFrames[i].pose.translation; keyFrame->mvFarFrames[i].pose.setEssentialMatrix(); } return; }*/ void LocalBundleAdjustment::refineKeyFrameConnection(KeyFrame* keyFrame){ return; int num_keyframe=keyFrame->mConnectedKeyFrameMatches.size(); int num_point=keyFrame->mvLocalMapPoints.size(); if (num_keyframe==0||(keyFrame->nextKeyFramePtr!=NULL&&num_keyframe==1)) { return; } printf("%d %d\n",num_keyframe,num_point); std::vector param_cam(6*num_keyframe); std::vector param_invDepth(num_point); std::map keyFrameIndex; int f=0; for(map::iterator mit=keyFrame->mConnectedKeyFramePoses.begin(), mend=keyFrame->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ if (mit->first==keyFrame->nextKeyFramePtr) {//next keyframe's pose is by bundle continue; } keyFrameIndex[mit->first]=f; ceres::RotationMatrixToAngleAxis(&(mit->second.rotation(0)),¶m_cam[6*f]); Eigen::Vector3d translation=mit->second.rotation*mit->second.translation; param_cam[6*f+3]=-translation(0); param_cam[6*f+4]=-translation(1); param_cam[6*f+5]=-translation(2); f++; } std::vector depthFixed(num_point,true); for (int p=0;pmvLocalMapPoints.size();p++) { param_invDepth[p]=keyFrame->mvLocalMapPoints[p].invdepth; } ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005); for(map >::iterator mit=keyFrame->mConnectedKeyFrameMatches.begin(), mend=keyFrame->mConnectedKeyFrameMatches.end(); mit!=mend; mit++){ KeyFrame* connectedKeyFrame=mit->first; if (connectedKeyFrame==keyFrame->nextKeyFramePtr) { continue; } f=keyFrameIndex[connectedKeyFrame]; std::vector &matches=mit->second; for (int i=0;imvLocalMapPoints[i].isEstimated) { assert(0); std::vector points(2); std::vector rotations(2,Eigen::Matrix3d::Identity()); std::vector cameras(2); Transform transform=keyFrame->mConnectedKeyFramePoses[connectedKeyFrame]; points[0]=keyFrame->mvLocalMapPoints[i].norm; points[1]=keyFrame->mvLocalMapPoints[matches[i]].norm; points[1]=transform.rotation.transpose()*points[1]; cameras[0]=Eigen::Vector3d::Zero(); cameras[1]=transform.translation; //assert(keyFrame->mvLocalMapPoints[i].previousInvDepth==0.0); Eigen::Vector3d point3D=multiviewTriangulationLinear(points,rotations,cameras); keyFrame->mvLocalMapPoints[i].invdepth=1.0/point3D(2); keyFrame->mvLocalMapPoints[i].isEstimated=true; } assert(keyFrame->mvLocalMapPoints[i].isEstimated); Eigen::Vector3d keyFramePoint=keyFrame->mvLocalMapPoints[i].vec; Eigen::Vector3d framePoint=connectedKeyFrame->mvLocalMapPoints[matches[i]].vec; ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(keyFramePoint(0),keyFramePoint(1), framePoint(0),framePoint(1)); problem.AddResidualBlock(cost_function,loss_function,¶m_cam[6*f],¶m_invDepth[i]); } } for (int i=0;imvLocalMapPoints[i].isEstimated&&keyFrame->mvLocalMapPoints[i].measurementCount>0) { problem.SetParameterBlockConstant(¶m_invDepth[i]); } } ceres::Solver::Options options; options.linear_solver_type = ceres::SPARSE_SCHUR; options.minimizer_progress_to_stdout=false; options.logging_type=ceres::SILENT; ceres::Solver::Summary summary; ceres::Solve(options,&problem, &summary); for (int i=0;imvLocalMapPoints[i].isEstimated&&keyFrame->mvLocalMapPoints[i].measurementCount<=0) { keyFrame->mvLocalMapPoints[i].invdepth=param_invDepth[i]; keyFrame->mvLocalMapPoints[i].updateNormalAndDepth(); } } for(map::iterator mit=keyFrame->mConnectedKeyFramePoses.begin(), mend=keyFrame->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ if (mit->first==keyFrame->nextKeyFramePtr) { continue; } f=keyFrameIndex[mit->first]; ceres::AngleAxisToRotationMatrix(¶m_cam[6*f],&(mit->second.rotation(0))); mit->second.translation=-Eigen::Vector3d(¶m_cam[6*f+3]); mit->second.translation=mit->second.rotation.transpose()*mit->second.translation; mit->second.setEssentialMatrix(); } return; } int LocalBundleAdjustment::refineKeyFrameMatches(KeyFrame *keyFrame1,const KeyFrame *keyFrame2,Transform &transform, vector& matches12,vector& matches21){ int nMatches=0; ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(projErrorThres); double param_cam[6]; ceres::RotationMatrixToAngleAxis(&(transform.rotation(0)),¶m_cam[0]); Eigen::Vector3d translation=transform.rotation*transform.translation; param_cam[3]=-translation(0); param_cam[4]=-translation(1); param_cam[5]=-translation(2); vector param_invDepth(matches12.size()); std::vector depthFixed(matches12.size(),false); for (int i=0;imvLocalMapPoints[i].isEstimated) { std::vector points(2); std::vector rotations(2,Eigen::Matrix3d::Identity()); std::vector cameras(2); points[0]=keyFrame1->mvLocalMapPoints[i].norm; points[1]=keyFrame2->mvLocalMapPoints[matches12[i]].norm; points[1]=transform.rotation.transpose()*points[1]; cameras[0]=Eigen::Vector3d::Zero(); cameras[1]=transform.translation; Eigen::Vector3d point3D=multiviewTriangulationLinear(points,rotations,cameras); keyFrame1->mvLocalMapPoints[i].invdepth=1.0/point3D(2); }else{ depthFixed[i]=true; } param_invDepth[i]=keyFrame1->mvLocalMapPoints[i].invdepth; } for (int i=0;imvLocalMapPoints[i].vec; Eigen::Vector3d framePoint=keyFrame2->mvLocalMapPoints[matches12[i]].vec; ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(keyFramePoint(0),keyFramePoint(1), framePoint(0),framePoint(1)); problem.AddResidualBlock(cost_function,loss_function,param_cam,¶m_invDepth[i]); } } for (int i=0;iframeId,keyFrame2->frameId,nMatches); for (int i=0;imvLocalMapPoints[i].getPosition(); Eigen::Vector3d proj=transform.rotation*(point3D-transform.translation); proj/=proj(2); double error=(proj-keyFrame2->mvLocalMapPoints[matches12[i]].vec).norm(); if (error>projErrorThres||param_invDepth[i]<0) { if(!matches21.empty()){ matches21[matches12[i]]=-1; } matches12[i]=-1; nMatches--; }else if(!depthFixed[i]){ keyFrame1->mvLocalMapPoints[i].isEstimated=true; keyFrame1->mvLocalMapPoints[i].invdepth=param_invDepth[i]; keyFrame1->mvLocalMapPoints[i].updateNormalAndDepth(); } } ceres::AngleAxisToRotationMatrix(param_cam,&(transform.rotation(0))); transform.translation=-Eigen::Vector3d(¶m_cam[3]); transform.translation=transform.rotation.transpose()*transform.translation; transform.setEssentialMatrix(); return nMatches; } int PnPEstimator::estimate(KeyFrame* keyFrame1,KeyFrame* keyFrame2, const vector& matches,Transform& pose){ std::vector indices; std::vector points3d; std::vector points2d; indices.reserve(matches.size()); points3d.reserve(matches.size()); points2d.reserve(matches.size()); //std::cout<<"size "<20); for(int i=0;i=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){ cv::Point3f point3d; cv::Point2f point2d; Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition(); point3d.x=point3D(0); point3d.y=point3D(1); point3d.z=point3D(2); Eigen::Vector3d normalized3d=keyFrame2->mvLocalMapPoints[matches[i]].norm; point2d.x=normalized3d(0)/normalized3d(2); point2d.y=normalized3d(1)/normalized3d(2); points3d.push_back(point3d); points2d.push_back(point2d); indices.push_back(i); } } points3d.shrink_to_fit(); points2d.shrink_to_fit(); indices.shrink_to_fit(); int nInliers=indices.size(); cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1); cv::Mat rVec,tVec,rMat; cv::solvePnP(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec); cv::Rodrigues(rVec,rMat); for(int r1=0;r1<3;r1++){ for(int r2=0;r2<3;r2++){ pose.rotation(r1,r2)=rMat.at(r1,r2); } } tVec=-rMat.t()*tVec; assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1); pose.translation(0)=tVec.at(0); pose.translation(1)=tVec.at(1); pose.translation(2)=tVec.at(2); return nInliers; } int PnPEstimator::estimate(KeyFrame* keyFrame1,KeyFrame* keyFrame2, vector& matches12,vector& matches21,Transform& pose){ std::vector indices; std::vector points3d; std::vector points2d; indices.reserve(matches12.size()); points3d.reserve(matches12.size()); points2d.reserve(matches12.size()); //std::cout<<"size "<=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){ cv::Point3f point3d; cv::Point2f point2d; Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition(); point3d.x=point3D(0); point3d.y=point3D(1); point3d.z=point3D(2); Eigen::Vector3d normalized3d=keyFrame2->mvLocalMapPoints[matches12[i]].norm; point2d.x=normalized3d(0)/normalized3d(2); point2d.y=normalized3d(1)/normalized3d(2); points3d.push_back(point3d); points2d.push_back(point2d); indices.push_back(i); } } points3d.shrink_to_fit(); points2d.shrink_to_fit(); indices.shrink_to_fit(); int nInliers=indices.size(); cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1); cv::Mat rVec,tVec,rMat; std::vector status(points3d.size()); cv::solvePnPRansac(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec,false,1000,threshold,prob); cv::Rodrigues(rVec,rMat); for(int r1=0;r1<3;r1++){ for(int r2=0;r2<3;r2++){ pose.rotation(r1,r2)=rMat.at(r1,r2); } } tVec=-rMat.t()*tVec; assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1); pose.translation(0)=tVec.at(0); pose.translation(1)=tVec.at(1); pose.translation(2)=tVec.at(2); for (int i=0;i=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){ Eigen::Vector3d proj=keyFrame1->mvLocalMapPoints[i].getPosition(); proj=pose.rotation*(proj-pose.translation); proj/=proj(2); Eigen::Vector3d vec=keyFrame2->mvLocalMapPoints[matches12[i]].vec; Eigen::Vector3d error=proj-vec; if (error.norm()>threshold) { matches21[matches12[i]]=-1; matches12[i]=-1; nInliers--; } } } return nInliers; } } ================================================ FILE: GSLAM/Estimation.h ================================================ // // Optimization.h // GSLAM // // Created by ctang on 9/4/16. // Copyright © 2016 ctang. All rights reserved. // #include "KLT.h" #include "KeyFrame.h" namespace GSLAM { class Undistortion{ }; class RelativeOutlierRejection{ public: Eigen::Matrix3d K; Eigen::Matrix3d rotation; float theshold; float prob; double minViewAngle; double medViewAngle; vector* isValid; int relativeRansac(const KeyFrame* keyFrame, const KLT_FeatureList featureList); }; class RelativePoseEstimation{ public: bool rotationIsKnown; Eigen::Matrix3d rotation; Eigen::Vector3d translation; double medViewAngle; double minViewAngle; vector* isValid; int estimateRelativePose(const KeyFrame* keyFrame, const KLT_FeatureList featureList, std::vector &pVectors); void unitTest(); private: }; class LocalFactorization{ public: void process(KeyFrame *keyFrame); void iterativeRefine(KeyFrame *keyFrame); }; class LocalBundleAdjustment{ public: double projErrorThres; double viewAngleThres; void refinePoints(KeyFrame* keyFrame); void triangulate2(KeyFrame* keyFrame); void bundleAdjust(KeyFrame *keyFrame,bool cameraFixed=false); void triangulate(KeyFrame *keyFrame); void refineKeyFrameConnection(KeyFrame *keyFrame); int refineKeyFrameMatches(KeyFrame* keyFrame1,const KeyFrame* keyFrame2,Transform& transform, std::vector& matches12,std::vector& matches21); //void estimateFarFrames(KeyFrame* keyFrame); int BAIterations; }; class PnPEstimator{ public: int minCount; double threshold; double prob; int estimate(KeyFrame *keyFrame1,KeyFrame *keyFrame2, const vector& matches,Transform& pose); int estimate(KeyFrame *keyFrame1,KeyFrame *keyFrame2, vector& matches12,vector& matches21,Transform& pose); }; } ================================================ FILE: GSLAM/Frame.cpp ================================================ // // Frame.cpp // GSLAM // // Created by ctang on 9/4/16. // Copyright © 2016 ctang. All rights reserved. // #include "Frame.h" #include "opencv2/imgproc/imgproc.hpp" namespace GSLAM{ long unsigned int Frame::nNextId=0; bool Frame::mbInitialComputations=true; float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy; float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY; float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; Frame::Frame(cv::Mat &im, const double &timeStamp, ORBextractor* extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef) :mpORBvocabulary(voc),mpORBextractor(extractor), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()){ keyFramePtr=NULL; //im.copyTo(debugImage); #ifdef DEBUG //im.copyTo(debugImage); #endif // Exctract ORB (*mpORBextractor)(im,cv::Mat(),mvKeys,mDescriptors); N = mvKeys.size(); if(mvKeys.empty()) return; mvpLocalMapPoints = vector(N,static_cast(NULL)); UndistortKeyPoints(); // This is done for the first created Frame if(mbInitialComputations){ ComputeImageBounds(im); mfGridElementWidthInv=static_cast(FRAME_GRID_COLS) /static_cast(mnMaxX-mnMinX); mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS) /static_cast(mnMaxY-mnMinY); fx = K.at(0,0); fy = K.at(1,1); cx = K.at(0,2); cy = K.at(1,2); mbInitialComputations=false; } mnId=nNextId++; //Scale Levels Info mnScaleLevels = mpORBextractor->GetLevels(); mfScaleFactor = mpORBextractor->GetScaleFactor(); mfLogScaleFactor = std::log(mfScaleFactor); mvScaleFactors.resize(mnScaleLevels); mvLevelSigma2.resize(mnScaleLevels); mvScaleFactors[0]=1.0f; mvLevelSigma2[0]=1.0f; for(int i=1; i(N,false); ComputeBoW(); } bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY){ posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv); posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv); //Keypoint's coordinates are undistorted, which could cause to go out of the image if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS) return false; return true; } void Frame::ComputeImageBounds(const cv::Mat &im) { if(mDistCoef.at(0)!=0.0){ cv::Mat mat(4,2,CV_32F); mat.at(0,0)=0.0; mat.at(0,1)=0.0; mat.at(1,0)=im.cols; mat.at(1,1)=0.0; mat.at(2,0)=0.0; mat.at(2,1)=im.rows; mat.at(3,0)=im.cols; mat.at(3,1)=im.rows; // Undistort corners mat=mat.reshape(2); cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); mat=mat.reshape(1); mnMinX = min(mat.at(0,0),mat.at(2,0)); mnMaxX = max(mat.at(1,0),mat.at(3,0)); mnMinY = min(mat.at(0,1),mat.at(1,1)); mnMaxY = max(mat.at(2,1),mat.at(3,1)); }else{ mnMinX = 0.0f; mnMaxX = im.cols; mnMinY = 0.0f; mnMaxY = im.rows; } } void Frame::UndistortKeyPoints() { if(mDistCoef.at(0)==0.0){ mvKeysUn=mvKeys; return; } // Fill matrix with points cv::Mat mat(N,2,CV_32F); for(int i=0; i(i,0)=mvKeys[i].pt.x; mat.at(i,1)=mvKeys[i].pt.y; } // Undistort points mat=mat.reshape(2); cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); mat=mat.reshape(1); // Fill undistorted keypoint vector mvKeysUn.resize(N); for(int i=0; i(i,0); kp.pt.y=mat.at(i,1); mvKeysUn[i]=kp; } } void Frame::ComputeBoW(){ if(mBowVec.empty()){ vector vCurrentDesc = toDescriptorVector(mDescriptors); mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4); } } std::vector Frame::toDescriptorVector(const cv::Mat &Descriptors){ std::vector vDesc; vDesc.reserve(Descriptors.rows); for (int j=0;j Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel,vector &distances) const { vector vIndices; vIndices.reserve(N); distances.reserve(N); const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); if(nMinCellX>=FRAME_GRID_COLS) return vIndices; const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv)); if(nMaxCellX<0) return vIndices; const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv)); if(nMinCellY>=FRAME_GRID_ROWS) return vIndices; const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv)); if(nMaxCellY<0) return vIndices; const bool bCheckLevels = (minLevel>0) || (maxLevel>=0); for(int ix = nMinCellX; ix<=nMaxCellX; ix++) { for(int iy = nMinCellY; iy<=nMaxCellY; iy++) { const vector vCell = mGrid[ix][iy]; if(vCell.empty()) continue; for(size_t j=0, jend=vCell.size(); j=0) if(kpUn.octave>maxLevel) continue; } const float distx = kpUn.pt.x-x; const float disty = kpUn.pt.y-y; if(fabs(distx) (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #ifndef FRAME_H #define FRAME_H #include #include "Settings.h" #include "Transform.h" #include "KeyFrame.h" #include "./DBoW2/BowVector.h" #include "./DBoW2/FeatureVector.h" #include "ORBVocabulary.h" #include "ORBextractor.h" #include namespace GSLAM { #define FRAME_GRID_ROWS 54 #define FRAME_GRID_COLS 96 class LocalMapPoint; class KeyFrame; class Frame { public: Frame(cv::Mat &im, const double &timeStamp, ORBextractor* extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef); // Copy constructor. Frame(const Frame &frame); public: int mnId; KeyFrame* keyFramePtr; // Vocabulary used for relocalization. ORBVocabulary* mpORBvocabulary; // Feature extractor. The right is used only in the stereo case. ORBextractor* mpORBextractor; DBoW2::BowVector mBowVec; DBoW2::FeatureVector mFeatVec; int N; int NCoarse; cv::Mat mDescriptors; std::vector mvKeys; std::vector mvKeysUn; std::vector mvbOutlier; std::vector mvpLocalMapPoints; std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; // Frame timestamp. double mTimeStamp; //static members static float mnMinX; static float mnMaxX; static float mnMinY; static float mnMaxY; static bool mbInitialComputations; static long unsigned int nNextId; static float mfGridElementWidthInv; static float mfGridElementHeightInv; cv::Mat mK; static float fx; static float fy; static float cx; static float cy; static float invfx; static float invfy; cv::Mat mDistCoef; float mfOriginalScaleFacotr; // Scale pyramid info. int mnScaleLevels; float mfScaleFactor; float mfLogScaleFactor; vector mvScaleFactors; vector mvInvScaleFactors; vector mvLevelSigma2; vector mvInvLevelSigma2; bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY); void ComputeImageBounds(const cv::Mat &im); void UndistortKeyPoints(); void ComputeBoW(); vector toDescriptorVector(const cv::Mat &Descriptors); vector GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel,vector &distances) const; #ifdef DEBUG cv::Mat debugImage; #endif }; } #endif ================================================ FILE: GSLAM/Geometry.cpp ================================================ // // Geometry.cpp // GSLAM // // Created by ctang on 9/17/16. // Copyright © 2016 ctang. All rights reserved. // #include #include "Geometry.h" #include "ceres/ceres.h" Eigen::Vector3d multiviewTriangulationLinear(const std::vector &points, const std::vector &rotations, const std::vector &cameras){ Eigen::Vector3d point3D; int ncamera=points.size(); Eigen::MatrixX4d A=Eigen::MatrixX4d::Zero(3*ncamera,4); Eigen::MatrixX4d pLastRow=Eigen::MatrixX4d::Zero(ncamera,4); for(int i=0;i(0,0)=rotations[i]; P.col(3)=-rotations[i]*cameras[i]; A.block<3,4>(3*i,0)=crossMat*P; pLastRow.row(i)=P.row(2); } Eigen::Matrix4d AtA=A.transpose()*A; Eigen::JacobiSVD svd(AtA,Eigen::ComputeFullV); Eigen::Vector4d solution=svd.matrixV().col(3); point3D(0)=solution(0)/solution(3); point3D(1)=solution(1)/solution(3); point3D(2)=solution(2)/solution(3); Eigen::VectorXd product=pLastRow*solution; int positiveCount=0; for(int i=0;i0); } if(positiveCount& points, const std::vector& rotations, const std::vector& cameras){ }*/ double multiviewTriangulationDepth(const std::vector &points, const std::vector &rotations, const std::vector &cameras){ int ncameras=points.size()-1; Eigen::VectorXd A=Eigen::VectorXd(3*ncameras); Eigen::VectorXd B=Eigen::VectorXd(3*ncameras); for (int i=1;i<=ncameras;i++) { Eigen::Matrix3d crossMat; Eigen::Vector3d rotated=rotations[i].transpose()*points[i]; setCrossMatrix(crossMat,rotated); rotated=crossMat*points[0]; A.block<3,1>(3*(i-1),0)=rotated; B.block<3,1>(3*(i-1),0)=crossMat*cameras[i]; } double a=A.dot(A); double b=A.dot(B); return std::abs(b/a); } /*void multiviewTriangulationNonliear(double& depth, const std::vector& points, const std::vector& rotations, const std::vector& cameras){ }*/ inline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta, const double sintheta, const double wx, const double wy, const double wz){ Eigen::Matrix3d R; R(0,0) = costheta + wx*wx*(1 - costheta); R(1,0) = wz*sintheta + wx*wy*(1 - costheta); R(2,0) = -wy*sintheta + wx*wz*(1 - costheta); R(0,1) = wx*wy*(1 - costheta) - wz*sintheta; R(1,1) = costheta + wy*wy*(1 - costheta); R(2,1) = wx*sintheta + wy*wz*(1 - costheta); R(0,2) = wy*sintheta + wx*wz*(1 - costheta); R(1,2) = -wx*sintheta + wy*wz*(1 - costheta); R(2,2) = costheta + wz*wz*(1 - costheta); return R; } Eigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eigen::Vector3d &v2){ double cos12=v1.dot(v2)/(v1.norm()*v2.norm()); double sin12=sqrt(1-cos12*cos12); Eigen::Vector3d axis =v1.cross(v2); axis.normalize(); Eigen::Matrix3d R=AngleAxis2RotationMatrix(cos12,sin12,axis(0),axis(1),axis(2)); return R; } ================================================ FILE: GSLAM/Geometry.h ================================================ // // Geometry.hpp // GSLAM // // Created by ctang on 9/17/16. // Copyright © 2016 ctang. All rights reserved. // #ifndef Geometry_h #define Geometry_h #include "Eigen/Dense" #include inline void setCrossMatrix(Eigen::Matrix3d &crossMat,const Eigen::Vector3d& vector){ crossMat(0,0)=crossMat(1,1)=crossMat(2,2)=0.0; crossMat(0,1)= vector(2); crossMat(0,2)= -vector(1); crossMat(1,0)= -vector(2); crossMat(1,2)= vector(0); crossMat(2,0)= vector(1); crossMat(2,1)= -vector(0); } Eigen::Vector3d multiviewTriangulationLinear(const std::vector &points, const std::vector &rotations, const std::vector &cameras); double multiviewTriangulationDepth(const std::vector &points, const std::vector &rotations, const std::vector &cameras); void multiviewTriangulationNonlinear(double& depth, const std::vector &points, const std::vector &rotations, const std::vector &cameras); inline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta, const double sintheta, const double wx, const double wy, const double wz); Eigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eigen::Vector3d &v2); #endif /* Geometry_h */ ================================================ FILE: GSLAM/GlobalReconstruction.cpp ================================================ // // GlobalReconstruction.cpp // GSLAM // // Created by ctang on 9/28/16. // Copyright © 2016 ctang. All rights reserved. // //#include "opencv2/viz.hpp" #include "GlobalReconstruction.h" #include "KeyFrame.h" #include "Drawer.h" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "ceres/ceres.h" #include "theia/sfm/global_pose_estimation/robust_rotation_estimator.h" #include "theia/sfm/twoview_info.h" #include "theia/sfm/types.h" #include const bool disable_loop=true; namespace GSLAM { void GlobalReconstruction::getScaleConstraint(KeyFrame* keyFrame1, vector& scaleConstraints){ for(map >::iterator mit=keyFrame1->mConnectedKeyFrameMatches.begin(), mend=keyFrame1->mConnectedKeyFrameMatches.end(); mit!=mend; mit++){ KeyFrame* keyFrame2=mit->first; if (keyFrame2->mvLocalFrames.empty()) { continue; } Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2]; ScaleConstraint constraint; constraint.keyFrameIndex1=keyFrame1->mnId; constraint.keyFrameIndex2=keyFrame2->mnId; vector scales; vector& matches=mit->second; for (int i=0;i=0) { assert(keyFrame1->mvLocalMapPoints[i].isEstimated); if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated){ continue; } Eigen::Vector3d distance1=keyFrame1->mvLocalMapPoints[i].getPosition()-pose.translation; Eigen::Vector3d distance2=keyFrame2->mvLocalMapPoints[matches[i]].getPosition(); /*printf("%d %d %x %x\n",keyFrame1->outId,keyFrame2->outId, keyFrame1->mvLocalMapPoints[i].gMP,keyFrame2->mvLocalMapPoints[matches[i]].gMP); for(map::iterator mit=keyFrame1->mvLocalMapPoints[i].gMP->measurements.begin(), mend=keyFrame1->mvLocalMapPoints[i].gMP->measurements.end(); mit!=mend; mit++){ printf("%d %d\n",mit->first->outId,mit->second); } for(map::iterator mit=keyFrame2->mvLocalMapPoints[matches[i]].gMP->measurements.begin(), mend=keyFrame2->mvLocalMapPoints[matches[i]].gMP->measurements.end(); mit!=mend; mit++){ printf("%d %d\n",mit->first->outId,mit->second); }*/ //assert(keyFrame1->mvLocalMapPoints[i].gMP==keyFrame2->mvLocalMapPoints[matches[i]].gMP); double relativeScale=distance2.norm()/distance1.norm(); scales.push_back(relativeScale); } } if (scales.size()>=scaleThreshold) { sort(scales.begin(),scales.end()); constraint.value12=log(scales[scales.size()/2]); }else{ continue; } constraint.weight=1.0; scaleConstraints.push_back(constraint); } } void GlobalReconstruction::getRotationConstraint(KeyFrame* keyFrame1, vector& rotationConstraints){ for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(), mend=keyFrame1->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ KeyFrame* keyFrame2=mit->first; Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2]; RotationConstraint constraint; constraint.keyFrameIndex1=keyFrame1->mnId; constraint.keyFrameIndex2=keyFrame2->mnId; constraint.rotation12=pose.rotation; if(pose.scale==-1&&(keyFrame1->frameId<70||keyFrame2->frameId<70)){ continue; } //printf("add %d %d\n",constraint.keyFrameIndex1,constraint.keyFrameIndex2); rotationConstraints.push_back(constraint); } } void GlobalReconstruction::getTranslationConstraint(KeyFrame* keyFrame1, vector& translationConstraints){ for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(), mend=keyFrame1->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ KeyFrame* keyFrame2=mit->first; Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2]; TranslationConstraint constraint; constraint.keyFrameIndex1=keyFrame1->mnId; constraint.keyFrameIndex2=keyFrame2->mnId; constraint.rotation1=keyFrame1->pose.rotation; constraint.translation12=pose.translation/keyFrame1->scale; if(pose.scale==-1&&(keyFrame1->frameId<70||keyFrame2->frameId<70)){ continue; } translationConstraints.push_back(constraint); } } void GlobalReconstruction::getSIM3Constraint(KeyFrame* keyFrame1,vector& sim3Constraints){ for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(), mend=keyFrame1->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ KeyFrame* keyFrame2=mit->first; Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2]; SIM3Constraint constraint; constraint.keyFrameIndex1=keyFrame1->mnId; constraint.keyFrameIndex2=keyFrame2->mnId; constraint.rotation12=pose.rotation; constraint.translation12=pose.translation; constraint.weight=1.0; vector& matches=keyFrame1->mConnectedKeyFrameMatches[keyFrame2]; vector scales; for (int i=0;i=0) { assert(keyFrame1->mvLocalMapPoints[i].isEstimated); if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated){ continue; } Eigen::Vector3d distance1=keyFrame1->mvLocalMapPoints[i].getPosition()-pose.translation; Eigen::Vector3d distance2=keyFrame2->mvLocalMapPoints[matches[i]].getPosition(); double relativeScale=distance2.norm()/distance1.norm(); scales.push_back(relativeScale); } } if(scales.empty()){ continue; } sort(scales.begin(),scales.end()); constraint.scale12=scales[scales.size()/2]; sim3Constraints.push_back(constraint); } } void GlobalReconstruction::estimateScale(){ vector scaleIndex(keyFrames.size(),-1); vector scaleConstraints; for (int i=0;imnId==i); getScaleConstraint(keyFrames[i],scaleConstraints); } //printf("%d keyframe size\n",keyFrames.size()); int nScales=0; for (int i=0;i newScales; newScales.resize(nScales); for(int i=0;ilogScale; } } /*for (int i=0;ilogScale=newScales[scaleIndex[i]]; keyFrames[i]->scale=exp(keyFrames[i]->logScale); printf("%f\n",keyFrames[i]->scale); } static ofstream record("/Users/chaos/Desktop/debug/scales_error.txt"); for (int i=0;ilogScale-keyFrames[scaleConstraints[i].keyFrameIndex1]->logScale; diff=abs(diff-scaleConstraints[i].value12); int id1=keyFrames[scaleConstraints[i].keyFrameIndex1]->outId; int id2=keyFrames[scaleConstraints[i].keyFrameIndex2]->outId; //printf("%f %d %d\n",diff,id1,id2); record< &rotationIndex){ std::vector rotationConstraints(0); std::vector newRotations; int nRotations=0; rotationIndex.resize(keyFrames.size(),-1); for (int k=0;kpose.rotation); } if (rotationIndex[rotationConstraints[i].keyFrameIndex2]==-1) { rotationIndex[rotationConstraints[i].keyFrameIndex2]=nRotations; nRotations++; newRotations.push_back(keyFrames[rotationConstraints[i].keyFrameIndex2]->pose.rotation); } rotationConstraints[i].variableIndex1=rotationIndex[rotationConstraints[i].keyFrameIndex1]; rotationConstraints[i].variableIndex2=rotationIndex[rotationConstraints[i].keyFrameIndex2]; } } std::unordered_map view_pairs; std::unordered_map orientations; theia::RobustRotationEstimator::Options options; options.max_num_irls_iterations=100; options.max_num_l1_iterations=10; theia::RobustRotationEstimator rotation_estimator(options); for (int i=0;imvLocalFrames.back().pose.rotation*newRotations[i-1]; } for (int i=0;ipose.rotation=newRotations[rotationIndex[k]]; } std::cout<<"rotation end"< &rotationIndex){ vector rotationConstraints(0); vector newRotations(keyFrames.size()); for (int k=0;kpose.rotation; } RotationConstraint constraint; constraint.keyFrameIndex1=-1; constraint.keyFrameIndex2=0; constraint.variableIndex1=-1; constraint.variableIndex2=0; constraint.rotation1=Eigen::Matrix3d::Identity(); constraint.rotation12=Eigen::Matrix3d::Identity(); constraint.weight=1.0; rotationConstraints.push_back(constraint); globalRotationEstimation.maxOuterIterations=1000; globalRotationEstimation.maxInnerIterations=20; globalRotationEstimation.solve(rotationConstraints,newRotations); for (int i=1;ipose.rotation=newRotations[rotationIndex[k]]; } } void GlobalReconstruction::estimateTranslation(const vector &translationIndex){ vector translationConstraints; for (int k=0;k newTranslations(keyFrames.size(),Eigen::Vector3d::Zero()); globalTranslationEstimation.maxIterations=10000; globalTranslationEstimation.solve(translationConstraints,newTranslations); for (int i=1;ipose.translation=newTranslations[translationIndex[k]]; cout<pose.translation.transpose()<pose.translation-keyFrames[translationConstraints[i].keyFrameIndex1]->pose.translation); double diff=differror.norm(); int id1=keyFrames[translationConstraints[i].keyFrameIndex1]->outId; int id2=keyFrames[translationConstraints[i].keyFrameIndex2]->outId; record< bool operator()(const T* const camera, const T* const point, T* residuals) const { T transformed[3]; ceres::AngleAxisRotatePoint(camera,point,transformed); transformed[0]+=camera[3]; transformed[1]+=camera[4]; transformed[2]+=camera[5]; T predicted_x=transformed[0]/transformed[2]; T predicted_y=transformed[1]/transformed[2]; residuals[0]=predicted_x-(T)tracked_x; residuals[1]=predicted_y-(T)tracked_y; residuals[0]=residuals[0]; residuals[1]=residuals[1]; return true; } static ceres::CostFunction* Create(const double tracked_x, const double tracked_y) { return (new ceres::AutoDiffCostFunction(new GlobalError2(tracked_x,tracked_y))); }; double tracked_x; double tracked_y; }; void GlobalReconstruction::globalRefine(){ std::vector points; std::vector cameras; std::vector> pairCameraPoint; int nPoints=0; cameras.resize(6*keyFrames.size()); for (int i=0;ipose.rotation*keyFrame->pose.translation; ceres::RotationMatrixToAngleAxis(keyFrame->pose.rotation.data(), &cameras[6*i]); cameras[6*i+3]=translation(0); cameras[6*i+4]=translation(1); cameras[6*i+5]=translation(2); nPoints+=keyFrame->mvLocalMapPoints.size(); } points.resize(3*nPoints); nPoints=0; ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(0.003); for (int k=0;kmnId==k); for (int i=0;imvLocalMapPoints.size();i++) { if (keyFrame1->mvLocalMapPoints[i].isEstimated) { Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition(); point3D/=keyFrame1->scale; point3D=keyFrame1->pose.rotation.transpose()*point3D+keyFrame1->pose.translation; points[3*(nPoints+i)]=point3D(0); points[3*(nPoints+i)+1]=point3D(1); points[3*(nPoints+i)+2]=point3D(2); } } std::vector status(keyFrame1->mvLocalMapPoints.size(),false); for(std::map >::iterator mit=keyFrame1->mConnectedKeyFrameMatches.begin(), mend=keyFrame1->mConnectedKeyFrameMatches.end(); mit!=mend; mit++){ KeyFrame* keyFrame2=mit->first; if (keyFrame2->mvLocalFrames.empty()) { continue; } Transform transform=keyFrame1->mConnectedKeyFramePoses[keyFrame2]; if (transform.scale<-10.0&&disable_loop) { continue; } std::vector& matches=mit->second; assert(matches.size()==keyFrame1->mvLocalMapPoints.size()); for (int i=0;i=0) { assert(keyFrame1->mvLocalMapPoints[i].isEstimated); Eigen::Vector3d projection(&points[3*(nPoints+i)]); projection=keyFrame2->pose.rotation*(projection-keyFrame2->pose.translation); projection/=projection(2); projection-=keyFrame2->mvLocalMapPoints[matches[i]].vec; if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated&&projection.norm()>0.008){ continue; } ceres::CostFunction* cost_function = GlobalError2::Create(keyFrame2->mvLocalMapPoints[matches[i]].vec(0), keyFrame2->mvLocalMapPoints[matches[i]].vec(1)); problem.AddResidualBlock(cost_function,loss_function, &cameras[6*keyFrame2->mnId], &points[3*(nPoints+i)]); status[i]=true; } } } if(keyFrame1->nextKeyFramePtr!=NULL){ for(int i=0;imvLocalMapPoints.size();i++){ Eigen::Vector3d projection(&points[3*(nPoints+i)]); projection=keyFrame1->nextKeyFramePtr->pose.rotation*(projection-keyFrame1->nextKeyFramePtr->pose.translation); projection/=projection(2); projection-=(*keyFrame1->mvLocalMapPoints[i].vecs.back()); if (keyFrame1->mvLocalMapPoints[i].isEstimated &&keyFrame1->mvLocalMapPoints[i].vecs.back()!=NULL&&projection.norm()<0.008) { assert(keyFrame1->mvLocalMapPoints[i].vecs.size()==keyFrame1->mvLocalFrames.size()); Eigen::Vector3d projection=(*keyFrame1->mvLocalMapPoints[i].vecs.back()); projection/=projection(2); ceres::CostFunction* cost_function = GlobalError2::Create(projection(0),projection(1)); problem.AddResidualBlock(cost_function,loss_function, &cameras[6*keyFrame1->nextKeyFramePtr->mnId], &points[3*(nPoints+i)]); status[i]=true; } } } for (int i=0;imvLocalMapPoints.size();i++) { if (status[i]==true) { ceres::CostFunction* cost_function = GlobalError2::Create(keyFrame1->mvLocalMapPoints[i].vec(0), keyFrame1->mvLocalMapPoints[i].vec(1)); problem.AddResidualBlock(cost_function,loss_function, &cameras[6*keyFrame1->mnId], &points[3*(nPoints+i)]); keyFrame1->mvLocalMapPoints[i].isEstimated=true; }else{ keyFrame1->mvLocalMapPoints[i].isEstimated=false; } } nPoints+=keyFrame1->mvLocalMapPoints.size(); } ceres::Solver::Options options; options.linear_solver_type = ceres::SPARSE_SCHUR; options.minimizer_progress_to_stdout = true; options.max_num_iterations=100; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); for(int i=0;ipose.rotation=rotation; keyFrames[i]->pose.translation=translation; } nPoints=0; for (int k=0;k scales; for (int i=0;imvLocalMapPoints.size();i++) { if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) { keyFrames[k]->mvLocalMapPoints[i].globalPosition(0)=points[3*(nPoints+i)]; keyFrames[k]->mvLocalMapPoints[i].globalPosition(1)=points[3*(nPoints+i)+1]; keyFrames[k]->mvLocalMapPoints[i].globalPosition(2)=points[3*(nPoints+i)+2]; Eigen::Vector3d globalDistance=keyFrames[k]->mvLocalMapPoints[i].globalPosition -keyFrames[k]->pose.translation; Eigen::Vector3d localDistance=keyFrames[k]->mvLocalMapPoints[i].getPosition(); scales.push_back(localDistance.norm()/globalDistance.norm()); } } std::sort(scales.begin(),scales.end()); keyFrames[k]->scale=scales[scales.size()/2]; nPoints+=keyFrames[k]->mvLocalMapPoints.size(); } for(int i=1;ipose.rotation=keyFrames[i]->pose.rotation*keyFrames[0]->pose.rotation.transpose(); keyFrames[i]->pose.translation=keyFrames[0]->pose.rotation*(keyFrames[i]->pose.translation -keyFrames[0]->pose.translation); } for (int k=0;kmvLocalMapPoints.size();i++) { if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) { keyFrames[k]->mvLocalMapPoints[i].globalPosition=keyFrames[0]->pose.rotation* (keyFrames[k]->mvLocalMapPoints[i].globalPosition-keyFrames[0]->pose.translation); } } } keyFrames[0]->pose.rotation=Eigen::Matrix3d::Identity(); keyFrames[0]->pose.translation=Eigen::Vector3d::Zero(); } void GlobalReconstruction::addNewKeyFrame(KeyFrame* keyFrame){ keyFrames.push_back(keyFrame); } void GlobalReconstruction::savePly(){ ofstream results(path+std::string("/keyframes.txt")); for (int k=0;kframeId<<' '<pose.translation.transpose(); for (int i1=0;i1<3;i1++) { for (int i2=0;i2<3;i2++) { results<<' '<pose.rotation(i1,i2); } } results<mvLocalFrames.size();i++) { Eigen::Matrix3d rotation; rotation=keyFrames[k]->mvLocalFrames[i].pose.rotation*keyFrames[k]->pose.rotation; Eigen::Vector3d translation=keyFrames[k]->mvLocalFrames[i].pose.translation; translation=keyFrames[k]->pose.rotation.transpose()*translation; translation/=keyFrames[k]->scale; translation+=keyFrames[k]->pose.translation; trajectories<mvLocalFrames[i].frameId<<' '<mvLocalMapPoints.size();i++) { keyFrames[k]->mvLocalMapPoints[i].isEstimated=false; } for(map >::iterator mit=keyFrames[k]->mConnectedKeyFrameMatches.begin(), mend=keyFrames[k]->mConnectedKeyFrameMatches.end(); mit!=mend; mit++){ for (int i=0;isecond.size();i++) { if (mit->second[i]>=0) { keyFrames[k]->mvLocalMapPoints[i].isEstimated=true; npoints++; } } } } ofstream pointcloud(path+std::string("/pointcloud.ply")); pointcloud << "ply" << '\n' << "format ascii 1.0" << '\n' << "element vertex " <mvLocalMapPoints.size();i++) { if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){ Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition(); point3D/=keyFrames[k]->scale; point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation; uchar* color=keyFrames[k]->mvLocalMapPoints[i].color; pointcloud <=keyFrames.back()->mvLocalFrames.back().frameId){ break; } cv::Mat image; if(drawer.mFrameIndexmvLocalFrames.back().frameId){ drawer.getCurrentOpenGLCameraMatrix(Twc); // char name[200]; // sprintf(name,"%s/1080/frame%05d.pgm",path,drawer.mFrameIndex+frameStart+1); // printf(name); // image=cv::imread(name); // printf("image %d %d\n",image.cols,image.rows); // if ((drawer.mFrameIndex)==keyFrames[drawer.mKeyFrameIndex]->frameId) { // cv::RNG rng(-1); // for(int i=0;imvLocalMapPoints.size();i++){ // if(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount<=0){ // continue; // } // //printf("%f %f %f\n",color.val[0],color.val[1],color[2]); // uchar *color=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].color; // // int Size=min(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount,20); // Size=max(Size,3); // cv::circle(image, // keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[0],Size, // CV_RGB(245,211,40),2,CV_AA); // // } // }else{ // for(int i=0;imvLocalMapPoints.size();i++){ // // if(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount<=0){ // continue; // } // // // if((drawer.mFrameIndex-keyFrames[drawer.mKeyFrameIndex]->frameId) // >=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked.size()){ // continue; // } // int localIndex=drawer.mFrameIndex-keyFrames[drawer.mKeyFrameIndex]->frameId; // // int Size=min(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount,20); // Size=max(Size,3); // uchar *color=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].color; // cv::circle(image,keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[localIndex],Size, // CV_RGB(245,211,40),2,CV_AA); // // for (int j=localIndex;j>max(localIndex-5,0);j--) { // cv::line(image, // keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[j], // keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[j-1], // CV_RGB(245,211,40),2,CV_AA); // } // } // } // cv::resize(image,image,cv::Size(720,405)); // image.copyTo(preImage); s_cam.Follow(Twc); }else{ preImage.copyTo(image); s_cam.Follow(Twc); } glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); glClearColor(1.0f,1.0f,1.0f,1.0f); drawer.drawCurrentCamera(Twc); drawer.drawKeyFrames(); drawer.drawPoints(); // char name[200]; // sprintf(name,"%s/viewer/viewr%05d",path,drawer.mFrameIndex); // pangolin::SaveWindowOnRender(name); // sprintf(name,"%s/viewer/track%05d.png",path,drawer.mFrameIndex); // cv::imwrite(name,image); // image.release(); pangolin::FinishFrame(); } } bool myfunction (const LocalMapPoint* p1,const LocalMapPoint* p2) { return (p1->invdepthinvdepth); } void GlobalReconstruction::topview(){ /*for (int k=0;k localMapPoints; for (int i=0;imvLocalMapPoints.size();i++) { if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) { localMapPoints.push_back(&keyFrames[k]->mvLocalMapPoints[i]); } } sort(localMapPoints.begin(),localMapPoints.end(),myfunction); for (int i=0;i<(0.8*localMapPoints.size());i++) { localMapPoints[i]->isEstimated=false; } }*/ //getchar(); //pangolin::WindowInterface& window=pangolin::CreateWindowAndBind("GSLAM: Map Viewer",720,720); //window.Move(100,480); // 3D Mouse handler requires depth testing to be enabled //glEnable(GL_DEPTH_TEST); // Issue specific OpenGl we might need //glEnable (GL_BLEND); //glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175)); float mViewpointX=0; float mViewpointY=0; float mViewpointZ=-1.8; float mViewpointF=500; for(int i=0;ipose.translation(0); mViewpointY+=keyFrames[i]->pose.translation(1); } mViewpointX/=keyFrames.size(); mViewpointY/=keyFrames.size(); float maxdistance=0.0; for (int i=1;ipose.translation.norm(); if (distance>maxdistance) { maxdistance=distance; } } printf("%f %f\n",mViewpointX,mViewpointY); // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000), pangolin::ModelViewLookAt(0, -maxdistance, 0, 0,0.0,0, pangolin::AxisDirection::AxisZ)); // Add named OpenGL viewport to window and provide 3D Handler pangolin::View& d_cam = pangolin::CreateDisplay() .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f) .SetHandler(new pangolin::Handler3D(s_cam)); pangolin::OpenGlMatrix Twc; Twc.SetIdentity(); bool bFollow = true; bool bLocalizationMode = false; float mT = 1e3/30.0; Drawer drawer; drawer.mCameraSize=0.5; drawer.mCameraLineWidth=3; drawer.mFrameIndex=0; drawer.mKeyFrameIndex=0; drawer.keyFrames=keyFrames; drawer.mGraphLineWidth=5.0; cv::namedWindow("track"); cv::moveWindow("track",720,0); drawer.preTwc.resize(1); drawer.preTwc[0]; drawer.preTwc[0].m[12]=0.0; drawer.preTwc[0].m[13]=0.0; drawer.preTwc[0].m[14]=0.0; pangolin::OpenGlMatrix curretMatrix; while(1){ if(drawer.mFrameIndexmvLocalFrames.back().frameId){ drawer.getCurrentOpenGLCameraMatrix(Twc); }else{ break; } curretMatrix=s_cam.GetModelViewMatrix(); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); glClearColor(1.0f,1.0f,1.0f,1.0f); drawer.drawCurrentCamera(Twc); drawer.drawKeyFrames(); drawer.drawPoints(); char name[200]; //sprintf(name,"%s/viewer/top%05d",path,drawer.mFrameIndex); //pangolin::SaveWindowOnRender(name); pangolin::FinishFrame(); } drawer.mFrameIndex=0; drawer.mKeyFrameIndex=0; drawer.preTwc.resize(1); drawer.preTwc[0]; drawer.preTwc[0].m[12]=0.0; drawer.preTwc[0].m[13]=0.0; drawer.preTwc[0].m[14]=0.0; while(1){ if(drawer.mFrameIndexmvLocalFrames.back().frameId){ drawer.getCurrentOpenGLCameraMatrix(Twc); } s_cam.SetModelViewMatrix(curretMatrix); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); glClearColor(1.0f,1.0f,1.0f,1.0f); drawer.drawCurrentCamera(Twc); drawer.drawKeyFrames(); drawer.drawPoints(); // char name[200]; // sprintf(name,"%s/viewer/top%05d",path,drawer.mFrameIndex); // pangolin::SaveWindowOnRender(name); pangolin::FinishFrame(); } } } ================================================ FILE: GSLAM/GlobalReconstruction.h ================================================ // // GlobalReconstruction.h // GSLAM // // Created by ctang on 9/8/16. // Copyright © 2016 ctang. All rights reserved. // #ifndef GlobalReconstruction_h #define GlobalReconstruction_h #include #include "Eigen/Dense" #include "ceres/rotation.h" namespace GSLAM{ inline Eigen::Vector3d ComputeRelativeRotationError(const Eigen::Matrix3d& relative_rotation_matrix, const Eigen::Matrix3d& rotation_matrix1, const Eigen::Matrix3d& rotation_matrix2) { // Compute the relative rotation error. const Eigen::Matrix3d relative_rotation_matrix_error =rotation_matrix2.transpose()*relative_rotation_matrix*rotation_matrix1; Eigen::Vector3d relative_rotation_error; ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(relative_rotation_matrix_error.data()), relative_rotation_error.data()); return relative_rotation_error; } inline void ApplyRotation(const Eigen::Vector3d& rotation_change,Eigen::Matrix3d& rotation_matrix) { // Convert to rotation matrices. Eigen::Matrix3d rotation_change_matrix; ceres::AngleAxisToRotationMatrix(rotation_change.data(),ceres::ColumnMajorAdapter3x3(rotation_change_matrix.data())); // Apply the rotation change. rotation_matrix = rotation_matrix*rotation_change_matrix; } inline int max3(int a, int b, int c){ int m = a; (m < b) && (m = b); //these are not conditional statements. (m < c) && (m = c); //these are just boolean expressions. return m; } typedef struct{ int variableIndex1; int variableIndex2; int keyFrameIndex1; int keyFrameIndex2; double value1; double value12; double weight; bool isLoop; }ScaleConstraint; typedef struct{ int variableIndex1; int variableIndex2; int keyFrameIndex1; int keyFrameIndex2; Eigen::Matrix3d rotation1;//if 1 is fixed Eigen::Matrix3d rotation12; double weight; bool isLoop; }RotationConstraint; typedef struct{ int variableIndex1; int variableIndex2; int keyFrameIndex1; int keyFrameIndex2; Eigen::Matrix3d rotation1; Eigen::Vector3d translation1;//if 1 is fixed Eigen::Vector3d translation12; double weight; bool isLoop; }TranslationConstraint; typedef struct{ int variableIndex1; int variableIndex2; int keyFrameIndex1; int keyFrameIndex2; Eigen::Matrix3d rotation12; Eigen::Vector3d translation12; double scale12; double weight; bool isLoop; }SIM3Constraint; class GlobalScaleEstimation{ public: double *objective; int *rowStart; int *column; double *rowUpper; double *rowLower; double *columnUpper; double *columnLower; double *elementByRow; int maxIterations; GlobalScaleEstimation(){ objective=NULL; rowStart=NULL; column=NULL; rowUpper=NULL; rowLower=NULL; columnUpper=NULL; columnLower=NULL; elementByRow=NULL; } void solve(const std::vector &constraints,std::vector &results); void test(); }; class GlobalRotationEstimation{ public: double *objective; int *rowStart; int *column; double *rowUpper; double *rowLower; double *columnUpper; double *columnLower; double *elementByRow; int maxOuterIterations; int maxInnerIterations; GlobalRotationEstimation(){ objective=NULL; rowStart=NULL; column=NULL; rowUpper=NULL; rowLower=NULL; columnUpper=NULL; columnLower=NULL; elementByRow=NULL; } void solve(const std::vector &constraints,std::vector& results); void test(); private: }; class GlobalTranslationEstimation{ public: double *objective; int *rowStart; int *column; double *rowUpper; double *rowLower; double *columnUpper; double *columnLower; double *elementByRow; int maxIterations; GlobalTranslationEstimation(){ objective=NULL; rowStart=NULL; column=NULL; rowUpper=NULL; rowLower=NULL; columnUpper=NULL; columnLower=NULL; elementByRow=NULL; } void solve(const std::vector &constraints,std::vector& results); void solveComplex(const std::vector &constraints,std::vector& results); void test(); }; class KeyFrame; class GlobalReconstruction{ std::vector keyFrames; GlobalRotationEstimation globalRotationEstimation; GlobalScaleEstimation globalScaleEstimation; GlobalTranslationEstimation globalTranslationEstimation; void getScaleConstraint(KeyFrame* keyFrame1,std::vector& scaleConstraints); void getRotationConstraint(KeyFrame* keyFrame1,std::vector& rotationConstraints); void getTranslationConstraint(KeyFrame* keyFrame1,std::vector& translationConstraints); void getSIM3Constraint(KeyFrame* keyFrame1,std::vector& constraints); public: void saveTUM(const char* savename,const std::vector& timestamps); void globalRefine(); char *path; int frameStart; GlobalReconstruction(){ keyFrames.clear(); } int scaleThreshold; void addNewKeyFrame(KeyFrame* keyFrame); void estimateScale(); void estimateRotation(std::vector& rotationIndex); void estimateRotationRobust(const std::vector& rotationIndex); void estimateTranslation(const std::vector& translationIndex); void estimateSIM3(); void savePly(); void visualize(); void topview(); }; } #endif /* GlobalReconstruction_h */ ================================================ FILE: GSLAM/GlobalRotationEstimation.cpp ================================================ // // GlobalRotationEstimation.cpp // GSLAM // // Created by ctang on 10/2/16. // Copyright © 2016 ctang. All rights reserved. // #include "GlobalReconstruction.h" #include "L1Solver.h" #include "opencv2/core/core.hpp" #include namespace GSLAM{ void GlobalRotationEstimation::solve(const std::vector& constraints, std::vector& results){ static const double kConvergenceThreshold = 1e-2; int variableCount=results.size(); int constraintCount=constraints.size(); int rowCount=2*constraintCount; int columnCount=variableCount+constraintCount; int singleVariableConstraintCount=0; for(int i=0;i rotationErrors(constraintCount); std::vector rotationUpdates(variableCount); double rotationError=0.0; for (int iter=0;iter globalRotations(num_rotations); std::vector constraints; cv::RNG rng(-1); for (int i=0;inum_rotations-100) { constraint.variableIndex1=0; constraint.variableIndex2=i; constraint.rotation12=globalRotations[i]*globalRotations[0].transpose(); constraint.weight=1.0; constraints.push_back(constraint); } } for (int i=0;i results(6,Eigen::Matrix3d::Identity()); int i=0; std::vector incrementalConstraints; for (int f=5;fsolve(incrementalConstraints,results); }else{ std::vector wx,wy,wz; for (int j=incrementalConstraints.size()-1;j>0;j--) { if (incrementalConstraints[j].variableIndex2==f) { const Eigen::Matrix3d rotationf=incrementalConstraints[j].rotation12*results[incrementalConstraints[j].variableIndex1]; double angles[3]; ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(rotationf.data()),angles); wx.push_back(angles[0]); wy.push_back(angles[1]); wz.push_back(angles[2]); } } std::sort(wx.begin(),wx.end()); std::sort(wy.begin(),wy.end()); std::sort(wz.begin(),wz.end()); const double angles[3]={wx[wx.size()/2],wy[wy.size()/2],wz[wz.size()/2]}; Eigen::Matrix3d rotationf; ceres::AngleAxisToRotationMatrix(angles,ceres::ColumnMajorAdapter3x3(rotationf.data())); results.push_back(rotationf); this->solve(incrementalConstraints,results); } } std::vector results2(num_rotations,Eigen::Matrix3d::Identity()); this->solve(constraints,results2); double error0=0.0,error1=0.0,error2=0; for (int i=1;i& constraints,std::vector& results){ //timeval time; //gettimeofday(&time, NULL); //long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000); int variableCount=results.size(); int constraintCount=constraints.size(); int rowCount=2*constraintCount; int columnCount=variableCount+constraintCount; int singleVariableConstraintCount=0; for(int i=0;imaxIterations); model.loadProblem(byRow,columnLower,columnUpper,objective,rowLower,rowUpper); model.dual(); const double *solutionPtr=model.primalColumnSolution(); for (int i=0;i globalScales(num_scales); cv::RNG rng(-1); for (int i=0;i constraints; for (int i=0;inum_scales-100) { constraint.variableIndex1=-1; constraint.variableIndex2=i; constraint.value1=0.0; constraint.value12=std::log(globalScales[i]); constraint.weight=1.0; constraints.push_back(constraint); } if (i!=0) { continue; } constraint.variableIndex1=-1; constraint.variableIndex2=i; constraint.value1=0.0; constraint.value12=std::log(globalScales[i]); constraint.weight=1.0; constraints.push_back(constraint); } for (int i=0;i incrementalConstraints; std::vector results2(6,0); for (int f=5;fsolve(incrementalConstraints,results2); }else{ std::vector v; for (int j=incrementalConstraints.size()-1;j>0;j--) { if (incrementalConstraints[j].variableIndex2==f) { v.push_back(incrementalConstraints[j].value12+results2[incrementalConstraints[j].variableIndex1]); } } std::sort(v.begin(),v.end()); results2.push_back(v[v.size()/2]); this->maxIterations=1000; this->solve(incrementalConstraints,results2); } } this->maxIterations=10000; this->solve(incrementalConstraints,results2); this->maxIterations=10000; std::vector results(num_scales,0); this->solve(constraints,results); results2.resize(results.size()); for (int i=1;i &constraints,std::vector& results){ int variableCount=results.size(); int constraintCount=constraints.size(); int rowCount=2*constraintCount; int columnCount=variableCount+constraintCount; int singleVariableConstraintCount=0; for(int i=0;imaxIterations); for (int c=0;c<3;c++) { for (int i = 0; i &constraints,std::vector& results){ } void GlobalTranslationEstimation::test(){ int num_translation=500; std::vector globalTranslations(num_translation); std::vector globalRotations(num_translation); cv::RNG rng(-1); for (int i=0;i constraints; for (int i=0;i results(num_translation,Eigen::Vector3d::Zero()); this->solve(constraints,results); double error0=0; double error1=0; for (int i=0;idata.db); int i; memset(sa, 0, sizeof(sa)); memset(sb, 0, sizeof(sb)); for (i = 0; i < count; i++) { sa[0] += a[i].x*a[i].x; sa[1] += a[i].y*a[i].x; sa[2] += a[i].x; sa[6] += a[i].x*a[i].y; sa[7] += a[i].y*a[i].y; sa[8] += a[i].y; sa[12] += a[i].x; sa[13] += a[i].y; sa[14] += 1; sb[0] += a[i].x*b[i].x; sb[1] += a[i].y*b[i].x; sb[2] += b[i].x; sb[3] += a[i].x*b[i].y; sb[4] += a[i].y*b[i].y; sb[5] += b[i].y; } sa[21] = sa[0]; sa[22] = sa[1]; sa[23] = sa[2]; sa[27] = sa[6]; sa[28] = sa[7]; sa[29] = sa[8]; sa[33] = sa[12]; sa[34] = sa[13]; sa[35] = sa[14]; cvSolve(&A, &B, &MM, CV_SVD); } else { double sa[16], sb[4], m[4], *om = M->data.db; CvMat A = cvMat(4, 4, CV_64F, sa), B = cvMat(4, 1, CV_64F, sb); CvMat MM = cvMat(4, 1, CV_64F, m); int i; memset(sa, 0, sizeof(sa)); memset(sb, 0, sizeof(sb)); for (i = 0; i < count; i++) { sa[0] += a[i].x*a[i].x + a[i].y*a[i].y; sa[1] += 0; sa[2] += a[i].x; sa[3] += a[i].y; sa[4] += 0; sa[5] += a[i].x*a[i].x + a[i].y*a[i].y; sa[6] += -a[i].y; sa[7] += a[i].x; sa[8] += a[i].x; sa[9] += -a[i].y; sa[10] += 1; sa[11] += 0; sa[12] += a[i].y; sa[13] += a[i].x; sa[14] += 0; sa[15] += 1; sb[0] += a[i].x*b[i].x + a[i].y*b[i].y; sb[1] += a[i].x*b[i].y - a[i].y*b[i].x; sb[2] += b[i].x; sb[3] += b[i].y; } cvSolve(&A, &B, &MM, CV_SVD); om[0] = om[4] = m[0]; om[1] = -m[1]; om[3] = m[1]; om[2] = m[2]; om[5] = m[3]; } } static int EstimateRigidTransform(cv::AutoBuffer &good_idx, int &good_count, const CvArr* matA, const CvArr* matB, CvMat* matM, int full_affine) { const int COUNT = 15; const int WIDTH = 160, HEIGHT = 120; const int RANSAC_MAX_ITERS = 500; const int RANSAC_SIZE0 = 3; const double RANSAC_GOOD_RATIO = 0.8; cv::Ptr sA, sB; cv::AutoBuffer pA, pB; cv::AutoBuffer status; cv::Ptr gray; CvMat stubA, *A = cvGetMat(matA, &stubA); CvMat stubB, *B = cvGetMat(matB, &stubB); CvSize sz0, sz1; int cn, equal_sizes; int i, j, k, k1; int count_x, count_y, count = 0; double scale = 1; CvRNG rng = cvRNG(-1); double m[6] = { 0 }; CvMat M = cvMat(2, 3, CV_64F, m); good_count = 0; CvRect brect; if (!CV_IS_MAT(matM)) CV_Error(matM ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix"); if (!CV_ARE_SIZES_EQ(A, B)) CV_Error(CV_StsUnmatchedSizes, "Both input images must have the same size"); if (!CV_ARE_TYPES_EQ(A, B)) CV_Error(CV_StsUnmatchedFormats, "Both input images must have the same data type"); if (CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2) { count = A->cols*A->rows; CvMat _pA, _pB; pA.allocate(count); pB.allocate(count); _pA = cvMat(A->rows, A->cols, CV_32FC2, pA); _pB = cvMat(B->rows, B->cols, CV_32FC2, pB); cvConvert(A, &_pA); cvConvert(B, &_pB); } else CV_Error(CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type"); good_idx.allocate(count); if (count < RANSAC_SIZE0){ //printf("too small!\n"); return 0; } CvMat _pB = cvMat(1, count, CV_32FC2, pB); brect = cvBoundingRect(&_pB, 1); // RANSAC stuff: // 1. find the consensus for (k = 0; k < RANSAC_MAX_ITERS; k++) { //printf("ransac %d\n", k); int idx[RANSAC_SIZE0]; CvPoint2D32f a[3]; CvPoint2D32f b[3]; memset(a, 0, sizeof(a)); memset(b, 0, sizeof(b)); // choose random 3 non-complanar points from A & B for (i = 0; i < RANSAC_SIZE0; i++) { for (k1 = 0; k1 < RANSAC_MAX_ITERS; k1++) { idx[i] = cvRandInt(&rng) % count; for (j = 0; j < i; j++) { if (idx[j] == idx[i]) break; // check that the points are not very close one each other if (fabs(pA[idx[i]].x - pA[idx[j]].x) + fabs(pA[idx[i]].y - pA[idx[j]].y) < FLT_EPSILON) break; if (fabs(pB[idx[i]].x - pB[idx[j]].x) + fabs(pB[idx[i]].y - pB[idx[j]].y) < FLT_EPSILON) break; } if (j < i) continue; if (i + 1 == RANSAC_SIZE0) { // additional check for non-complanar vectors a[0] = pA[idx[0]]; a[1] = pA[idx[1]]; a[2] = pA[idx[2]]; b[0] = pB[idx[0]]; b[1] = pB[idx[1]]; b[2] = pB[idx[2]]; double dax1 = a[1].x - a[0].x, day1 = a[1].y - a[0].y; double dax2 = a[2].x - a[0].x, day2 = a[2].y - a[0].y; double dbx1 = b[1].x - b[0].x, dby1 = b[1].y - b[0].y; double dbx2 = b[2].x - b[0].x, dby2 = b[2].y - b[0].y; const double eps = 0.01; if (fabs(dax1*day2 - day1*dax2) < eps*sqrt(dax1*dax1 + day1*day1)*sqrt(dax2*dax2 + day2*day2) || fabs(dbx1*dby2 - dby1*dbx2) < eps*sqrt(dbx1*dbx1 + dby1*dby1)*sqrt(dbx2*dbx2 + dby2*dby2)) continue; } break; } if (k1 >= RANSAC_MAX_ITERS) break; } if (i < RANSAC_SIZE0) continue; // estimate the transformation using 3 points icvGetRTMatrix(a, b, 3, &M, full_affine); for (i = 0, good_count = 0; i < count; i++) { if (fabs(m[0] * pA[i].x + m[1] * pA[i].y + m[2] - pB[i].x) + fabs(m[3] * pA[i].x + m[4] * pA[i].y + m[5] - pB[i].y) < MAX(brect.width, brect.height)*0.005) good_idx[good_count++] = i; } if (good_count >= count*RANSAC_GOOD_RATIO) break; } if (k >= RANSAC_MAX_ITERS){ //printf("ransac_max_iters\n"); return 0; } if (good_count < count) { for (i = 0; i < good_count; i++) { j = good_idx[i]; pA[i] = pA[j]; pB[i] = pB[j]; } } icvGetRTMatrix(pA, pB, good_count, &M, full_affine); m[2] /= scale; m[5] /= scale; cvConvert(&M, matM); return 1; } static cv::Mat EstimateRigidTransform(cv::AutoBuffer &goodIdx, int &good_count, cv::InputArray src1, cv::InputArray src2, bool fullAffine){ cv::Mat M(2, 3, CV_64F), A = src1.getMat(), B = src2.getMat(); CvMat matA = A, matB = B, matM = M; int err = EstimateRigidTransform(goodIdx, good_count, &matA, &matB, &matM, fullAffine); if (err == 1){ return M; }else{ M.setTo(0); M.at(0, 0) = 1.0; M.at(1, 1) = 1.0; return M; } } static bool homographyRefineLM(const std::vector& pts1, const std::vector& pts2, cv::Mat& homography,const int maxIters){ CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON)); CvMat modelPart = cvMat(solver.param->rows, solver.param->cols,homography.type(),homography.ptr()); cvCopy( &modelPart, solver.param ); for(;;) { const CvMat* _param = 0; CvMat *_JtJ = 0, *_JtErr = 0; double* _errNorm = 0; if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm )) break; for(int i = 0; i data.db; double Mx = pts1[i].x, My = pts1[i].y; double ww = h[6]*Mx + h[7]*My + 1.; ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0; double _xi = (h[0]*Mx + h[1]*My + h[2])*ww; double _yi = (h[3]*Mx + h[4]*My + h[5])*ww; double err[] = { _xi - pts2[i].x, _yi - pts2[i].y }; if( _JtJ || _JtErr ) { double J[][8] = { { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi }, { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi } }; for(int j = 0; j < 8; j++ ) { for(int k = j; k < 8; k++ ) _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k]; _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1]; } } if( _errNorm ) *_errNorm += err[0]*err[0] + err[1]*err[1]; } } cvCopy(solver.param,&modelPart ); return true; } } namespace GSLAM{ static cv::Mat EstimateHomography( const std::vector& pts1, const std::vector& pts2, std::vector& status, int& good_count){ cv::AutoBuffer goodIdx; bool fullAffine=false; cv::Mat rigid=GSLAM::EstimateRigidTransform(goodIdx,good_count,pts1,pts2,fullAffine); cv::Mat homography=cv::Mat::eye(3,3,CV_64F); for (int r=0;r<2;r++) { for (int c=0;c<3;c++) { homography.at(r,c)=rigid.at(r,c); } } std::vector _pts1(good_count),_pts2(good_count); for (int i=0;i(pts1.size(),false); for (int i=0;i motions; std::vector gaps; void getDiffRotation(cv::Mat &rotation, const int startGyroIndex, const int endGyroIndex, const double startStamp, const double endStamp){ double dt; if (startGyroIndex==endGyroIndex) { dt=endStamp-startStamp; rotation=rotation*getSkewRotation(motions[startGyroIndex].anglev * dt); }else{ dt = motions[startGyroIndex+1].stamp-startStamp; rotation=rotation*getSkewRotation(motions[startGyroIndex].anglev * dt); for (int i=startGyroIndex+1;i0) { motionIndex--; } } void synchronization(std::vector &frameStamps,double ts){ int frameCount = frameStamps.size(); while (frameStamps[frameCount - 1] + ts>motions[motions.size()-1].stamp) { frameCount--; } frameStamps.resize(frameCount); } void getIntraFrameRotation(std::vector &rotations, const std::vector &originVertices, cv::Size imageSize, cv::Size sizeByVertex, int &globalIndex, double &globalStamp){ //printf("%d %f\n",globalIndex,globalStamp);getchar(); updateMotionIndex(globalStamp,globalIndex); rotations[0]=cv::Mat::eye(3,3,CV_64FC1); int preIndex=globalIndex; double preStamp=globalStamp; int rowIndex=globalIndex; double rowStamp=0; int rowVertexIndex=0; for (int i=1;i preparedGridVertices; std::vector warpedGridVertices; void initializeGridVertices(cv::Size gridSize,cv::Size sizeByVertex){ originGridVertices.reserve(sizeByVertex.width*sizeByVertex.height); preparedGridVertices.resize(sizeByVertex.width*sizeByVertex.height); warpedGridVertices.resize(sizeByVertex.width*sizeByVertex.height); for (int i=0;i originGridVertices; cv::Size sizeByGrid; cv::Size sizeByVertex; cv::Size gridSize; const std::vector& getOriginGridVertices() const {return originGridVertices;}; const std::vector& getPreparedGridVertices() const {return preparedGridVertices;}; std::vector& getWarpedGridVertices() {return warpedGridVertices;}; void initialize(cv::Size _gridSize,cv::Size _sizeByGrid,cv::Size _sizeByVertex){ initializeGridVertices(_gridSize,_sizeByVertex); sizeByGrid=_sizeByGrid; sizeByVertex=_sizeByVertex; gridSize=_gridSize; } void rotateAndNormalizePoint(const cv::Point2f &src, cv::Mat &dst, const std::vector &rotations, const cv::Mat &invK){ double T[9]; cv::Mat transform(3,3,CV_64FC1,T); int h0=src.y/gridSize.height; int h1=h0+1; int vIndex0=h0*sizeByVertex.width; int vIndex1=vIndex0+sizeByVertex.width; double dist0=src.y-originGridVertices[vIndex0].y; double dist1=originGridVertices[vIndex1].y-src.y; double dist =dist0+dist1; dist0=dist1/dist; dist1=1-dist0; transform=dist0*rotations[h0]+dist1*rotations[h1]; transform=transform.t()*invK; dst=cv::Mat(3,1,CV_64FC1); double *data=(double*)dst.data; data[0]= T[0]*src.x + T[1]*src.y + T[2]; data[1]= T[3]*src.x + T[4]*src.y + T[5]; data[2]= T[6]*src.x + T[7]*src.y + T[8]; double normValue=cv::norm(dst,cv::NORM_L2); dst*=(1.0/normValue); return; } void prepareMeshByRow(const std::vector &transformArray){ double T[9]; cv::Mat transform(3,3,CV_64FC1,T); for (int h=0;h #pragma once #define USE_TBB #define SSE_TRACKING typedef struct { float lighting_alpha; float lighting_beta; /* Available to user */ int mindist; /* min distance b/w features */ int window_width, window_height; bool sequentialMode; /* whether to save most recent image to save time */ /* can set to TRUE manually, but don't set to */ /* FALSE manually */ bool smoothBeforeSelecting; /* whether to smooth image before */ /* selecting features */ bool writeInternalImages; /* whether to write internal images */ /* tracking features */ bool lighting_insensitive; /* whether to normalize for gain and bias (not in original algorithm) */ /* Available, but hopefully can ignore */ int min_eigenvalue; /* smallest eigenvalue allowed for selecting */ float min_determinant; /* th for determining lost */ float min_displacement; /* th for stopping tracking when pixel changes little */ int max_iterations; /* th for stopping tracking when too many iterations */ float max_residue; /* th for stopping tracking when residue is large */ float grad_sigma; float smooth_sigma_fact; float pyramid_sigma_fact; float step_factor; /* size of Newton steps; 2.0 comes from equations, 1.0 seems to avoid overshooting */ int nSkippedPixels; /* # of pixels skipped when finding features */ int borderx; /* border in which features will not be found */ int bordery; int nPyramidLevels; /* computed from search_ranges */ int featureSelectionLevel; int subsampling; /* " */ /* for affine mapping */ int affine_window_width, affine_window_height; int affineConsistencyCheck; /* whether to evaluates the consistency of features with affine mapping -1 = don't evaluates the consistency 0 = evaluates the consistency of features with translation mapping 1 = evaluates the consistency of features with similarity mapping 2 = evaluates the consistency of features with affine mapping */ int affine_max_iterations; float affine_max_residue; float affine_min_displacement; float affine_max_displacement_differ; /* th for the difference between the displacement calculated by the affine tracker and the frame to frame tracker in pel*/ } KLT_TrackingContextRec, *KLT_TrackingContext; #define KLT_TRACKED 0 #define KLT_NOT_FOUND -1 #define KLT_SMALL_DET -2 #define KLT_MAX_ITERATIONS -3 #define KLT_OOB -4 #define KLT_LARGE_RESIDUE -5 typedef struct { float x; float y; int val; cv::Point2f pt; Eigen::Vector3d norm; Eigen::Vector3d vec; float aff_x; float aff_y; float aff_Axx; float aff_Ayx; float aff_Axy; float aff_Ayy; cv::Mat *aff_img; cv::Mat *aff_gradx; cv::Mat *aff_grady; cv::Mat coefficient; } KLT_FeatureRec,*KLT_Feature; typedef struct { bool isOutlierRejected; int nFeatures; KLT_Feature *feature; } KLT_FeatureListRec, *KLT_FeatureList; /* Kernels */ #define MAX_KERNEL_WIDTH 71 typedef struct { int width; float data[MAX_KERNEL_WIDTH]; } ConvolutionKernel; static ConvolutionKernel gauss_kernel; static ConvolutionKernel gaussderiv_kernel; static float sigma_last = -10.0; static int KLT_verbose = 0; typedef float KLT_locType; typedef unsigned char KLT_PixelType; ================================================ FILE: GSLAM/KLTUtil.cpp ================================================ // // KLT.cpp // GSLAM // // Created by ctang on 9/8/16. // Copyright © 2016 ctang. All rights reserved. // #include "KLTUtil.h" #include "error.h" static const int _mindist = 10; static const int _window_size = 21; static const int _min_eigenvalue = 200; static const float _min_determinant = 0.01f; static const float _min_displacement = 0.1f; static const int _max_iterations = 10; static const float _max_residue = 10.0f; static const float _grad_sigma = 1.0f; static const float _smooth_sigma_fact = 0.1f; static const float _pyramid_sigma_fact = 0.9f; //static const float _grad_sigma = 0.5f; //static const float _smooth_sigma_fact = 0.1f; //static const float _pyramid_sigma_fact = 0.3f; static const float _step_factor = 1.0f; static const bool _sequentialMode = false; static const bool _lighting_insensitive = false; /* for affine mapping*/ static const int _affineConsistencyCheck = -1; static const int _affine_window_size = 15; static const int _affine_max_iterations = 10; static const float _affine_max_residue = 10.0; static const float _affine_min_displacement = 0.02f; static const float _affine_max_displacement_differ = 1.5f; static const bool _smoothBeforeSelecting = true; static const bool _writeInternalImages = false; static const int _search_range = 15; static const int _nSkippedPixels = 0; KLT_FeatureList KLTCreateFeatureList(KLT_TrackingContext tc,int nFeatures) { KLT_FeatureList fl; KLT_Feature first; int nbytes = sizeof(KLT_FeatureListRec) + 100*nFeatures * sizeof(KLT_Feature) + 100*nFeatures * sizeof(KLT_FeatureRec); int i; /* Allocate memory for feature list */ fl = (KLT_FeatureList)malloc(nbytes); /* Set parameters */ fl->nFeatures = nFeatures; /* Set pointers */ fl->feature = (KLT_Feature *) (fl + 1); first = (KLT_Feature) (fl->feature + 100*nFeatures); for (i = 0 ; i <100*nFeatures ; i++) { fl->feature[i] = first + i; fl->feature[i]->aff_x=-1.0; fl->feature[i]->aff_y=-1.0; fl->feature[i]->aff_img=NULL; fl->feature[i]->aff_gradx=NULL; fl->feature[i]->aff_grady=NULL; } /* Return feature list */ return(fl); } void KLTChangeTCPyramid(KLT_TrackingContext tc, int search_range) { float window_halfwidth; float subsampling; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("(KLTChangeTCPyramid) Window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("(KLTChangeTCPyramid) Window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("(KLTChangeTCPyramid) Window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("(KLTChangeTCPyramid) Window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } window_halfwidth = std::min(tc->window_width,tc->window_height)/2.0f; subsampling = ((float) search_range) / window_halfwidth; if (subsampling < 1.0) { /* 1.0 = 0+1 */ tc->nPyramidLevels = 1; } else if (subsampling <= 3.0) { /* 3.0 = 2+1 */ tc->nPyramidLevels = 2; tc->subsampling = 2; } else if (subsampling <= 5.0) { /* 5.0 = 4+1 */ tc->nPyramidLevels = 2; tc->subsampling = 4; } else if (subsampling <= 9.0) { /* 9.0 = 8+1 */ tc->nPyramidLevels = 2; tc->subsampling = 8; } else { float val = (float) (log(7.0*subsampling+1.0)/log(8.0)); tc->nPyramidLevels = (int) (val + 0.99); tc->subsampling = 8; } } void computeKernels(float sigma, ConvolutionKernel *gauss, ConvolutionKernel *gaussderiv) { const float factor = 0.01f; /* for truncating tail */ int i; assert(MAX_KERNEL_WIDTH % 2 == 1); assert(sigma >= 0.0); /* Compute kernels, and automatically determine widths */ { const int hw = MAX_KERNEL_WIDTH / 2; float max_gauss = 1.0f, max_gaussderiv = (float) (sigma*exp(-0.5f)); /* Compute gauss and deriv */ for (i = -hw ; i <= hw ; i++) { gauss->data[i+hw] = (float) exp(-i*i / (2*sigma*sigma)); gaussderiv->data[i+hw] = -i * gauss->data[i+hw]; } /* Compute widths */ gauss->width = MAX_KERNEL_WIDTH; for (i = -hw ; fabs(gauss->data[i+hw] / max_gauss) < factor ; i++, gauss->width -= 2); gaussderiv->width = MAX_KERNEL_WIDTH; for (i = -hw ; fabs(gaussderiv->data[i+hw] / max_gaussderiv) < factor ; i++, gaussderiv->width -= 2); if (gauss->width == MAX_KERNEL_WIDTH || gaussderiv->width == MAX_KERNEL_WIDTH) KLTError("(_computeKernels) MAX_KERNEL_WIDTH %d is too small for " "a sigma of %f", MAX_KERNEL_WIDTH, sigma); } /* Shift if width less than MAX_KERNEL_WIDTH */ for (i = 0 ; i < gauss->width ; i++) gauss->data[i] = gauss->data[i+(MAX_KERNEL_WIDTH-gauss->width)/2]; for (i = 0 ; i < gaussderiv->width ; i++) gaussderiv->data[i] = gaussderiv->data[i+(MAX_KERNEL_WIDTH-gaussderiv->width)/2]; /* Normalize gauss and deriv */ { const int hw = gaussderiv->width / 2; float den; den = 0.0; for (i = 0 ; i < gauss->width ; i++) den += gauss->data[i]; for (i = 0 ; i < gauss->width ; i++) gauss->data[i] /= den; den = 0.0; for (i = -hw ; i <= hw ; i++) den += i*gaussderiv->data[i+hw]; for (i = -hw ; i <= hw ; i++) gaussderiv->data[i+hw] /= den; } sigma_last = sigma; } /********************************************************************* * _KLTGetKernelWidths * */ void _KLTGetKernelWidths(float sigma, int *gauss_width, int *gaussderiv_width){ computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); *gauss_width = gauss_kernel.width; *gaussderiv_width = gaussderiv_kernel.width; } float _KLTComputeSmoothSigma(KLT_TrackingContext tc){ return (tc->smooth_sigma_fact * std::max(tc->window_width, tc->window_height)); } static float _pyramidSigma(KLT_TrackingContext tc){ return (tc->pyramid_sigma_fact * tc->subsampling); } void KLTUpdateTCBorder(KLT_TrackingContext tc){ float val; int pyramid_gauss_hw; int smooth_gauss_hw; int gauss_width, gaussderiv_width; int num_levels = tc->nPyramidLevels; int n_invalid_pixels; int window_hw; int ss = tc->subsampling; int ss_power; int border; int i; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("(KLTUpdateTCBorder) Window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("(KLTUpdateTCBorder) Window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("(KLTUpdateTCBorder) Window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("(KLTUpdateTCBorder) Window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } window_hw = std::max(tc->window_width, tc->window_height)/2; /* Find widths of convolution windows */ _KLTGetKernelWidths(_KLTComputeSmoothSigma(tc), &gauss_width, &gaussderiv_width); smooth_gauss_hw = gauss_width/2; _KLTGetKernelWidths(_pyramidSigma(tc), &gauss_width, &gaussderiv_width); pyramid_gauss_hw = gauss_width/2; /* Compute the # of invalid pixels at each level of the pyramid. n_invalid_pixels is computed with respect to the ith level of the pyramid. So, e.g., if n_invalid_pixels = 5 after the first iteration, then there are 5 invalid pixels in level 1, which translated means 5*subsampling invalid pixels in the original level 0. */ n_invalid_pixels = smooth_gauss_hw; for (i = 1 ; i < num_levels ; i++) { val = ((float) n_invalid_pixels + pyramid_gauss_hw) / ss; n_invalid_pixels = (int) (val + 0.99); /* Round up */ } /* ss_power = ss^(num_levels-1) */ ss_power = 1; for (i = 1 ; i < num_levels ; i++) ss_power *= ss; /* Compute border by translating invalid pixels back into */ /* original image */ border = (n_invalid_pixels + window_hw) * ss_power; tc->borderx = border; tc->bordery = border; } KLT_TrackingContext KLTCreateTrackingContext(){ KLT_TrackingContext tc; /* Allocate memory */ tc = (KLT_TrackingContext) malloc(sizeof(KLT_TrackingContextRec)); /* Set values to default values */ tc->mindist = _mindist; tc->window_width = _window_size; tc->window_height = _window_size; tc->sequentialMode = _sequentialMode; tc->smoothBeforeSelecting = _smoothBeforeSelecting; tc->writeInternalImages = _writeInternalImages; tc->lighting_insensitive = _lighting_insensitive; tc->min_eigenvalue = _min_eigenvalue; tc->min_determinant = _min_determinant; tc->max_iterations = _max_iterations; tc->min_displacement = _min_displacement; tc->max_residue = _max_residue; tc->grad_sigma = _grad_sigma; tc->smooth_sigma_fact = _smooth_sigma_fact; tc->pyramid_sigma_fact = _pyramid_sigma_fact; tc->step_factor = _step_factor; tc->nSkippedPixels = _nSkippedPixels; /* for affine mapping */ tc->affineConsistencyCheck = _affineConsistencyCheck; tc->affine_window_width = _affine_window_size; tc->affine_window_height = _affine_window_size; tc->affine_max_iterations = _affine_max_iterations; tc->affine_max_residue = _affine_max_residue; tc->affine_min_displacement = _affine_min_displacement; tc->affine_max_displacement_differ = _affine_max_displacement_differ; /* Change nPyramidLevels and subsampling */ KLTChangeTCPyramid(tc,_search_range); /* Update border, which is dependent upon */ /* smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling */ KLTUpdateTCBorder(tc); return(tc); } ================================================ FILE: GSLAM/KLTUtil.h ================================================ // // KLTUtil.cpp // GSLAM // // Created by ctang on 9/8/16. // Copyright © 2016 ctang. All rights reserved. // #include "KLT.h" #pragma once void computeKernels(float sigma,ConvolutionKernel *gauss,ConvolutionKernel *gaussderiv); float _KLTComputeSmoothSigma(KLT_TrackingContext tc); KLT_TrackingContext KLTCreateTrackingContext(); KLT_FeatureList KLTCreateFeatureList(KLT_TrackingContext tc,int nFeatures); ================================================ FILE: GSLAM/KeyFrame.cpp ================================================ // // KeyFrame.cpp // GSLAM // // Created by ctang on 9/4/16. // Copyright © 2016 ctang. All rights reserved. // #include "KeyFrame.h" #include "Estimation.h" #include "Geometry.h" #include "MapPoint.h" //#include "Drawer.h" //#include //#include "opencv2/viz.hpp" namespace GSLAM{ long unsigned int KeyFrame::nNextId=0; KeyFrame::KeyFrame(Frame *F,KeyFrameDatabase *pKFDB,const Eigen::Matrix3d &invK,double _mScaleFactor): mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mBowVec(F->mBowVec), mFeatVec(F->mFeatVec),mpKeyFrameDB(pKFDB), mpORBvocabulary(F->mpORBvocabulary),mScaleFactor(_mScaleFactor){ mnId=nNextId++; framePtr=F; F->keyFramePtr=this; //initialize for geometry mvRelativeEstimated.resize(framePtr->mvKeys.size()); std::fill(mvRelativeEstimated.begin(),mvRelativeEstimated.end(),true); mvLocalFrames.clear(); mvLocalMapPoints.resize(framePtr->mvKeys.size()); for (int i=0;imvKeys[i].pt.x*mScaleFactor, (double)framePtr->mvKeys[i].pt.y*mScaleFactor,1.0); kpt=invK*kpt; //mvLocalMapPoints[i].pt=framePtr->mvKeys[i].pt*mScaleFactor; mvLocalMapPoints[i].norm=kpt; mvLocalMapPoints[i].norm.normalize(); mvLocalMapPoints[i].vec=kpt; mvLocalMapPoints[i].vec/=mvLocalMapPoints[i].vec(2); mvLocalMapPoints[i].vecs.clear(); mvLocalMapPoints[i].pVectors.clear(); mvLocalMapPoints[i].isFullTrack=true; mvLocalMapPoints[i].isEstimated=false; mvLocalMapPoints[i].measurementCount=0; const int level = framePtr->mvKeysUn[i].octave; const int nLevels = framePtr->mnScaleLevels; mvLocalMapPoints[i].maxLevelScaleFactor=framePtr->mvScaleFactors[level]; mvLocalMapPoints[i].minLevelScaleFactor=framePtr->mvScaleFactors[nLevels-1]; #ifdef DEBUG //std::cout<mBowVec), mFeatVec(F->mFeatVec),mpKeyFrameDB(pKFDB), mpORBvocabulary(F->mpORBvocabulary){ mnId=nNextId++; framePtr=F; F->keyFramePtr=this; //initialize for geometry mvRelativeEstimated.resize(featureList->nFeatures); std::fill(mvRelativeEstimated.begin(),mvRelativeEstimated.end(),true); mvLocalFrames.clear(); mvLocalMapPoints.resize(featureList->nFeatures); for (int i=0;ifeature[i]->x,featureList->feature[i]->y,1.0); kpt=invK*kpt; mvLocalMapPoints[i].norm=kpt; mvLocalMapPoints[i].norm.normalize(); mvLocalMapPoints[i].vec=kpt; mvLocalMapPoints[i].vec/=mvLocalMapPoints[i].vec(2); //mvLocalMapPoints[i].pt.x=featureList->feature[i]->x; //mvLocalMapPoints[i].pt.y=featureList->feature[i]->y; mvLocalMapPoints[i].vecs.clear(); mvLocalMapPoints[i].pVectors.clear(); mvLocalMapPoints[i].isFullTrack=true; mvLocalMapPoints[i].isEstimated=false; mvLocalMapPoints[i].measurementCount=0; #ifdef DEBUG //std::cout< KeyFrame::GetConnectedKeyFrames(){ unique_lock lock(mMutexConnections); set s; for(map::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++) s.insert(mit->first); return s; } vector KeyFrame::GetBestCovisibilityKeyFrames(const int &N){ unique_lock lock(mMutexConnections); if((int)mvpOrderedConnectedKeyFrames.size()(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N); } int KeyFrame::appendRelativeEstimation(const int frameId,KeyFrame *keyFrame, const Eigen::Matrix3d &rotation,const Eigen::Vector3d &translation, const KLT_FeatureList featureList,const vector &pVectors){ LocalFrame frame; frame.frameId=frameId; frame.measurementCount=0; frame.pose.rotation=rotation; frame.pose.translation=translation; int fullTrackCount=0; for (int i=0;ifeature[i]->val==KLT_TRACKED) { Eigen::Vector3d* vecPtr=new Eigen::Vector3d(featureList->feature[i]->vec); mvLocalMapPoints[i].vecs.push_back(vecPtr); mvLocalMapPoints[i].measurementCount++; frame.measurementCount++; }else{ mvLocalMapPoints[i].vecs.push_back(static_cast(NULL)); } } mvLocalFrames.push_back(frame); return fullTrackCount; } void KeyFrame::UpdateBestCovisibles(){ vector > vPairs; vPairs.reserve(mConnectedKeyFrameWeights.size()); for(map::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++){ vPairs.push_back(make_pair(mit->second,mit->first)); } sort(vPairs.begin(),vPairs.end()); list lKFs; list lWs; for(size_t i=0, iend=vPairs.size(); i(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector(lWs.begin(), lWs.end()); } int KeyFrame::appendKeyFrame(KeyFrame *keyFrame,Transform transform,vector& matches){ int nInlierMatch=0; for (int i=0;i=0); } assert(!mConnectedKeyFrameWeights.count(keyFrame)); printf("append %d to %d %d\n",this->frameId,keyFrame->frameId,nInlierMatch); if (nInlierMatch>=20) { if(!mConnectedKeyFrameWeights.count(keyFrame)){ mConnectedKeyFrameWeights[keyFrame]=nInlierMatch; mConnectedKeyFramePoses[keyFrame]=transform; mConnectedKeyFrameMatches[keyFrame].resize(matches.size()); std::copy(matches.begin(),matches.end(),mConnectedKeyFrameMatches[keyFrame].begin()); } else if(mConnectedKeyFrameWeights[keyFrame]!=nInlierMatch){ mConnectedKeyFrameWeights[keyFrame]=nInlierMatch; mConnectedKeyFramePoses[keyFrame]=transform; std::copy(matches.begin(),matches.end(),mConnectedKeyFrameMatches[keyFrame].begin()); } else{ return 0; } UpdateBestCovisibles(); }else{ assert(0); } //printf("%d append %d %d\n",frameId,keyFrame->frameId,nInlierMatch); //finally append frame /*for (int i=0;i=0) { printf("keyframe %d %d estimated matched keyframe %d %d estimated\n", frameId,i,mvLocalMapPoints[i].isEstimated, keyFrame->frameId,matches[i],keyFrame->mvLocalMapPoints[matches[i]].isEstimated); } }*/ return nInlierMatch; } void KeyFrame::savePly(const char* savename){ int num_point=0; for(int i=0;i::iterator mit=mConnectedKeyFramePoses.begin(), mend=mConnectedKeyFramePoses.end(); mit!=mend; mit++){ Transform pose=mit->second; if (pose.scale==-1.0) { of< (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #ifndef KEYFRAME_H #define KEYFRAME_H #include "./DBoW2/BowVector.h" #include "./DBoW2/FeatureVector.h" #include "ORBVocabulary.h" #include "Frame.h" #include "KLT.h" #include "MapPoint.h" #include "KeyFrameDatabase.h" #include #include namespace GSLAM{ class KeyFrameDatabase; class LocalFrame{ public: int measurementCount; int frameId; Transform pose; }; class KeyFrame{ public: std::thread baThread; KeyFrame(Frame *F,KeyFrameDatabase* pKFDB,const Eigen::Matrix3d &invK,double mScaleFactor); KeyFrame(Frame *F,KeyFrameDatabase* pKFDB,const Eigen::Matrix3d &invK,KLT_FeatureList featureList); KeyFrame* prevKeyFramePtr; KeyFrame* nextKeyFramePtr; Frame* framePtr; //Keyframe Index static long unsigned int nNextId; int mnId; int frameId; int outId; // MapPoints associated to keypoints KLT_FeatureList mFeatureList; std::vector mvLocalFrames; std::vector mvLocalMapPoints; std::vector mvRelativeEstimated; std::vector > trackedPoints; // BoW KeyFrameDatabase* mpKeyFrameDB; ORBVocabulary* mpORBvocabulary; DBoW2::BowVector mBowVec; DBoW2::FeatureVector mFeatVec; float mScaleFactor; Eigen::Matrix3d invK; Eigen::Matrix3d frameK; // Variables used by the keyframe database long unsigned int mnLoopQuery; int mnLoopWords; float mLoopScore; long unsigned int mnRelocQuery; int mnRelocWords; float mRelocScore; float minScore; // Covisibility std::set GetConnectedKeyFrames(); std::vector GetBestCovisibilityKeyFrames(const int &N); std::map mConnectedKeyFramePoses; std::map > mConnectedKeyFrameMatches; std::map mConnectedKeyFrameWeights; std::vector mvpOrderedConnectedKeyFrames; std::vector mvOrderedWeights; // Global graph property Transform pose; double scale; double logScale; bool isGlobalFixed; // mutex std::mutex mMutexPose; std::mutex mMutexConnections; std::mutex mMutexFeatures; //return full track count int appendRelativeEstimation(const int frameId,KeyFrame* keyFrame, const Eigen::Matrix3d &rotation,const Eigen::Vector3d &translation, const KLT_FeatureList featureList,const vector &pVectors); int appendKeyFrame(KeyFrame* nextKeyFrame,Transform transform,vector& matches); void savePly(const char* name); void saveData2(const char* name); void visualize(); private: void UpdateBestCovisibles(); }; } #endif ================================================ FILE: GSLAM/KeyFrameConnection.cpp ================================================ // // KeyFrameConnection.cpp // GSLAM // // Created by ctang on 9/24/16. // Copyright © 2016 ctang. All rights reserved. // #include "KeyFrameConnection.h" #include "Estimation.h" namespace GSLAM{ void updateScore(ORBVocabulary* mpORBVocabulary,KeyFrame* keyFrame1,KeyFrame* keyFrame2){ float score=mpORBVocabulary->score(keyFrame1->mBowVec,keyFrame2->mBowVec); if (scoreminScore) { keyFrame1->minScore=score; } if (scoreminScore) { keyFrame2->minScore=score; } } int KeyFrameConnection::connectLoop(KeyFrame* keyFrame1,KeyFrame* keyFrame2, Transform& transform12, std::vector &matches12, std::vector &matches21){ matches12.resize(keyFrame1->mvLocalMapPoints.size(),-1); matches21.resize(keyFrame2->mvLocalMapPoints.size(),-1); int nMatches12=matcherByBoW->SearchByBoW(keyFrame1,keyFrame2,matches12,matches21); printf("loop match0 %d %d %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches12); if (nMatches12>connectionThreshold) { PnPEstimator pnp; pnp.prob=0.99; pnp.threshold=0.01; nMatches12=pnp.estimate(keyFrame1,keyFrame2,matches12,matches21,transform12); printf("loop pnp %d %d %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches12); LocalBundleAdjustment localBA; localBA.projErrorThres=0.008; std::vector errors; localBA.BAIterations=5; nMatches12=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2, transform12,matches12,matches21); printf("loop refine1 %d %d %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches12); nMatches12=matcherByProjection->SearchByProjection(keyFrame1,keyFrame2,transform12,matches12,matches21,3,64); nMatches12=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,transform12, matches12,matches21); printf("loop refine2 %d %d %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches12); transform12.scale=-1.0; } return nMatches12; } void KeyFrameConnection::connectLoop(KeyFrame* keyFrame2){ vector vpCandidateKFs = keyFrameDatabase->DetectLoopCandidates(keyFrame2,keyFrame2->minScore*0.8); for (int i=0;iframeId,keyFrame2->frameId,keyFrame1->minScore,keyFrame2->minScore); assert(!keyFrame1->mConnectedKeyFramePoses.count(keyFrame2) &&!keyFrame2->mConnectedKeyFramePoses.count(keyFrame1)); Transform transform12; std::vector matches12,matches21; int nMatches12=connectLoop(keyFrame1,keyFrame2,transform12,matches12,matches21); Transform transform21; std::vector _matches21,_matches12; int nMatches21=connectLoop(keyFrame2,keyFrame1,transform21,_matches21,_matches12); //printf("loop matched %d %d %d %d %f %f\n",keyFrame1->frameId,keyFrame2->frameId,nMatches12,nMatches21,transform12.scale,transform21.scale); //printf("thres %d\n",connectionThreshold); if (nMatches12>connectionThreshold||nMatches21>connectionThreshold) { LocalBundleAdjustment localBA; localBA.BAIterations=5; localBA.projErrorThres=0.08; for (int i=0;i=0&&_matches12[i]<0) { _matches21[matches12[i]]=i; //printf("%d %d added %d\n",keyFrame2->frameId,matches12[i],i); } } for (int i=0;i<_matches21.size();i++) { if (_matches21[i]>=0&&matches21[i]<0) { matches12[_matches21[i]]=i; //printf("%d %d added %d\n",keyFrame1->frameId,_matches21[i],i); } } nMatches12=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,transform12,matches12,matches21); nMatches21=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,transform21,_matches21,_matches12); //cout<frameId,keyFrame2->frameId,nMatches12,nMatches21); if(nMatches12>connectionThreshold/2&&nMatches21>connectionThreshold/2){ keyFrame1->appendKeyFrame(keyFrame2,transform12,matches12); localBA.refineKeyFrameConnection(keyFrame1); updateScore(mpORBVocabulary,keyFrame1,keyFrame2); //printf("loop connected %d %d %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches12); //} //if (nMatches21>connectionThreshold/2&&nMatches12>connectionThreshold/2) { keyFrame2->appendKeyFrame(keyFrame1,transform21,_matches21); localBA.refineKeyFrameConnection(keyFrame2); updateScore(mpORBVocabulary,keyFrame1,keyFrame2); //printf("loop connected %d %d %d\n",keyFrame2->frameId,keyFrame1->frameId,nMatches21); /*for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(), mend=keyFrame1->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ Transform transform32,transform23; std::vector matches32,matches23; std::vector _matches32,_matches23; int nMatches32=connectKeyFrame(mit->first,keyFrame1,keyFrame2,transform32,matches32,matches23); int nMatches23=connectKeyFrame(keyFrame2,keyFrame1,mit->first,transform23,_matches23,_matches32); if(nMatches32=0&&_matches32[i]<0) { _matches23[matches32[i]]=i; } } for (int i=0;i<_matches23.size();i++) { if (_matches23[i]>=0&&matches23[i]<0) { matches32[_matches23[i]]=i; } } localBA.BAIterations=5; nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23); nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32); if(nMatches32>connectionThreshold/2&&nMatches23>connectionThreshold/2){ mit->first->appendKeyFrame(keyFrame2,transform32,matches32); localBA.refineKeyFrameConnection(mit->first); keyFrame2->appendKeyFrame(mit->first,transform23,_matches23); localBA.refineKeyFrameConnection(keyFrame2); updateScore(mpORBVocabulary,keyFrame2,mit->first); } } for(map::iterator mit=keyFrame2->mConnectedKeyFramePoses.begin(), mend=keyFrame2->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ KeyFrame* keyFrame3=mit->first; for(map::iterator mit2=keyFrame3->mConnectedKeyFramePoses.begin(), mend2=keyFrame3->mConnectedKeyFramePoses.end(); mit2!=mend2; mit2++){ if (keyFrame2->mConnectedKeyFramePoses.count(keyFrame3)) { continue; } Transform transform32,transform23; std::vector matches32,matches23; std::vector _matches32,_matches23; int nMatches32=connectKeyFrame(mit2->first,keyFrame3,keyFrame2,transform32,matches32,matches23); int nMatches23=connectKeyFrame(keyFrame2,keyFrame3,mit2->first,transform23,_matches23,_matches32); if(nMatches32=0&&_matches32[i]<0) { _matches23[matches32[i]]=i; } } for (int i=0;i<_matches23.size();i++) { if (_matches23[i]>=0&&matches23[i]<0) { matches32[_matches23[i]]=i; } } localBA.BAIterations=5; nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23); nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32); mit->first->appendKeyFrame(keyFrame2,transform32,matches32); localBA.refineKeyFrameConnection(mit->first); keyFrame2->appendKeyFrame(mit->first,transform23,_matches23); localBA.refineKeyFrameConnection(keyFrame2); updateScore(mpORBVocabulary,keyFrame2,mit->first); } }*/ } } } } int KeyFrameConnection::connectKeyFrame(KeyFrame* keyFrame3,KeyFrame* keyFrame1,KeyFrame* keyFrame2, Transform& transform32,std::vector &matches32,std::vector &matches23){ if (keyFrame3==keyFrame2||keyFrame3->mConnectedKeyFramePoses.count(keyFrame2)) { return -1; } const Transform transform31=keyFrame3->mConnectedKeyFramePoses[keyFrame1]; const Transform transform13=keyFrame1->mConnectedKeyFramePoses[keyFrame3]; matches32.resize(keyFrame3->mvLocalMapPoints.size(),-1); matches23.resize(keyFrame2->mvLocalMapPoints.size(),-1); if (!keyFrame3->mConnectedKeyFrameMatches.count(keyFrame1) ||!keyFrame1->mConnectedKeyFrameMatches.count(keyFrame2)) { assert(0); return -1; } const std::vector& matches31=keyFrame3->mConnectedKeyFrameMatches[keyFrame1]; const std::vector& matches12=keyFrame1->mConnectedKeyFrameMatches[keyFrame2]; int nMatches32=0; for (int i=0;imvLocalMapPoints.size();i++) { int idx1=matches31[i]; if (idx1<0) { continue; } //printf("%d %d %d %d %d %d\n",keyFrame3->frameId,keyFrame1->frameId,keyFrame2->frameId,i,idx1,matches12[idx1]); int index2=matches12[idx1]; if (index2>=0) { assert(matches23[index2]==-1); matches32[i]=index2; matches23[index2]=i; nMatches32++; } } std::vector scales; for (int i=0;imvLocalMapPoints[matches31[i]].isEstimated){ continue; }else if(!keyFrame3->mvLocalMapPoints[i].isEstimated){ //printf("%d %d\n",keyFrame3->frameId,i); //std::cout<mvLocalMapPoints[i].measurementCount<mvLocalMapPoints[i].getPosition()-transform31.translation).norm(); double distance1=keyFrame1->mvLocalMapPoints[matches31[i]].getPosition().norm(); scales.push_back(distance3/distance1); distance3=keyFrame3->mvLocalMapPoints[i].getPosition().norm(); distance1=(keyFrame1->mvLocalMapPoints[matches31[i]].getPosition()-transform13.translation).norm(); scales.push_back(distance3/distance1); } double relativeScale; if (scales.size()>5) { std::sort(scales.begin(),scales.end()); relativeScale=scales[scales.size()/2]; }else{ relativeScale=1.0; } Transform transform12=keyFrame1->mConnectedKeyFramePoses[keyFrame2]; transform12.translation*=relativeScale; transform32=transform31.leftMultiply(transform12); LocalBundleAdjustment localBA; localBA.projErrorThres=0.008; localBA.BAIterations=5; printf("first pass"); nMatches32=localBA.refineKeyFrameMatches(keyFrame3,keyFrame2,transform32,matches32,matches23); nMatches32=matcherByProjection->SearchByProjection(keyFrame3,keyFrame2,transform32,matches32,matches23,3,64); printf("second pass"); nMatches32=localBA.refineKeyFrameMatches(keyFrame3,keyFrame2,transform32,matches32,matches23); return nMatches32; } void KeyFrameConnection::connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2, std::vector &matches21){ std::vector matches12; matches12.resize(keyFrame1->mvLocalMapPoints.size()); matches21.resize(keyFrame2->mvLocalMapPoints.size()); std::fill(matches12.begin(),matches12.end(),-1); std::fill(matches21.begin(),matches21.end(),-1); int nMatches1=matcherByTracking->SearchByTracking(keyFrame1,keyFrame2,matches12,matches21,80); int nMatches2=matcherByProjection->SearchByProjection(keyFrame1,keyFrame2,keyFrame1->mvLocalFrames.back().pose, matches12,matches21,3,64); LocalBundleAdjustment localBA; localBA.projErrorThres=0.008; Transform pose12=keyFrame1->mvLocalFrames.back().pose; localBA.BAIterations=5; int nMatches=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,pose12,matches12,matches21); } void KeyFrameConnection::connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2){ std::vector matches12,matches21; matches12.resize(keyFrame1->mvLocalMapPoints.size()); matches21.resize(keyFrame2->mvLocalMapPoints.size()); std::fill(matches12.begin(),matches12.end(),-1); std::fill(matches21.begin(),matches21.end(),-1); int nMatches1=matcherByTracking->SearchByTracking(keyFrame1,keyFrame2,matches12,matches21,80); int nMatches2=matcherByProjection->SearchByProjection(keyFrame1,keyFrame2,keyFrame1->mvLocalFrames.back().pose, matches12,matches21,3,64); LocalBundleAdjustment localBA; localBA.projErrorThres=0.008; Transform pose12=keyFrame1->mvLocalFrames.back().pose; localBA.BAIterations=5; int nMatches=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,pose12,matches12,matches21); /*for (int i=0;i0&&matches12[j]>0){ assert(matches12[i]!=matches12[j]); } } }*/ std::vector _matches12,_matches21; _matches12.resize(keyFrame1->mvLocalMapPoints.size()); _matches21.resize(keyFrame2->mvLocalMapPoints.size()); std::fill(_matches12.begin(),_matches12.end(),-1); std::fill(_matches21.begin(),_matches21.end(),-1); std::vector scales; for (int i=0;i=0&&keyFrame2->mvLocalMapPoints[matches12[i]].isEstimated) { _matches12[i]=matches12[i]; _matches21[_matches12[i]]=i; double distance2=keyFrame2->mvLocalMapPoints[matches12[i]].getPosition().norm(); double distance1=(keyFrame1->mvLocalMapPoints[i].getPosition() -keyFrame1->mvLocalFrames.back().pose.translation).norm(); scales.push_back(distance2/distance1); } } std::sort(scales.begin(),scales.end()); double scale=scales[scales.size()/2]; Transform pose21=pose12.inverse(); pose21.translation*=scale; localBA.BAIterations=5; int _nMatches2=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,pose21,_matches21,_matches12); _nMatches2=matcherByProjection->SearchByProjection(keyFrame2,keyFrame1,pose21,_matches21,_matches12,3,64); localBA.BAIterations=5; _nMatches2=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,pose21,_matches21,_matches12); /*printf("%d %d nMatches %d %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches,_nMatches2); for (int i=0;i0&&matches12[j]>0){ assert(matches12[i]!=matches12[j]); } } } for (int i=0;i<_matches21.size()-1;i++) { for (int j=i+1;j<_matches21.size();j++) { if(_matches21[i]>0&&_matches21[j]>0){ assert(_matches21[i]!=_matches21[j]); } } }*/ for (int i=0;i=0&&_matches12[i]<0) { _matches21[matches12[i]]=i; //printf("%d %d added %d\n",keyFrame2->frameId,matches12[i],i); } } for (int i=0;i<_matches21.size();i++) { if (_matches21[i]>=0&&matches21[i]<0) { matches12[_matches21[i]]=i; //printf("%d %d added %d\n",keyFrame1->frameId,_matches21[i],i); } } /*for (int i=0;i0&&matches12[j]>0){ assert(matches12[i]!=matches12[j]); } } } for (int i=0;i<_matches21.size()-1;i++) { for (int j=i+1;j<_matches21.size();j++) { if(_matches21[i]>0&&_matches21[j]>0){ //printf("%d %d %d\n",keyFrame2->frameId,_matches21[i],matches21[j]); assert(_matches21[i]!=_matches21[j]); } } }*/ localBA.BAIterations=5; nMatches=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,pose12,matches12,matches21); _nMatches2=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,pose21,_matches21,_matches12); keyFrame1->appendKeyFrame(keyFrame2,keyFrame1->mvLocalFrames.back().pose,matches12); keyFrame1->nextKeyFramePtr=keyFrame2; keyFrame2->appendKeyFrame(keyFrame1,pose21,_matches21); keyFrame2->prevKeyFramePtr=keyFrame1; updateScore(mpORBVocabulary,keyFrame1,keyFrame2); localBA.projErrorThres=0.008; //second layer: connect keyframe2 to keyframe1's connection for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(), mend=keyFrame1->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ Transform transform32,transform23; std::vector matches32,matches23; std::vector _matches32,_matches23; int nMatches32=connectKeyFrame(mit->first,keyFrame1,keyFrame2,transform32,matches32,matches23); int nMatches23=connectKeyFrame(keyFrame2,keyFrame1,mit->first,transform23,_matches23,_matches32); if(nMatches32=0&&_matches32[i]<0) { _matches23[matches32[i]]=i; } } for (int i=0;i<_matches23.size();i++) { if (_matches23[i]>=0&&matches23[i]<0) { matches32[_matches23[i]]=i; } } localBA.BAIterations=5; nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23); nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32); if(nMatches32>connectionThreshold/2&&nMatches23>connectionThreshold/2){ mit->first->appendKeyFrame(keyFrame2,transform32,matches32); localBA.refineKeyFrameConnection(mit->first); keyFrame2->appendKeyFrame(mit->first,transform23,_matches23); localBA.refineKeyFrameConnection(keyFrame2); updateScore(mpORBVocabulary,keyFrame2,mit->first); } } for(map::iterator mit=keyFrame2->mConnectedKeyFramePoses.begin(), mend=keyFrame2->mConnectedKeyFramePoses.end(); mit!=mend; mit++){ KeyFrame* keyFrame3=mit->first; for(map::iterator mit2=keyFrame3->mConnectedKeyFramePoses.begin(), mend2=keyFrame3->mConnectedKeyFramePoses.end(); mit2!=mend2; mit2++){ if (keyFrame2->mConnectedKeyFramePoses.count(keyFrame3)) { continue; } Transform transform32,transform23; std::vector matches32,matches23; std::vector _matches32,_matches23; int nMatches32=connectKeyFrame(mit2->first,keyFrame3,keyFrame2,transform32,matches32,matches23); int nMatches23=connectKeyFrame(keyFrame2,keyFrame3,mit2->first,transform23,_matches23,_matches32); if(nMatches32=0&&_matches32[i]<0) { _matches23[matches32[i]]=i; } } for (int i=0;i<_matches23.size();i++) { if (_matches23[i]>=0&&matches23[i]<0) { matches32[_matches23[i]]=i; } } localBA.BAIterations=5; nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23); nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32); mit->first->appendKeyFrame(keyFrame2,transform32,matches32); localBA.refineKeyFrameConnection(mit->first); keyFrame2->appendKeyFrame(mit->first,transform23,_matches23); localBA.refineKeyFrameConnection(keyFrame2); updateScore(mpORBVocabulary,keyFrame2,mit->first); } } connectionThreshold=80; connectLoop(keyFrame2); } } ================================================ FILE: GSLAM/KeyFrameConnection.h ================================================ // // KeyFrameConnection.hpp // GSLAM // // Created by ctang on 9/24/16. // Copyright © 2016 ctang. All rights reserved. // #ifndef KeyFrameConnection_hpp #define KeyFrameConnection_hpp #include "KeyFrame.h" #include "ORBmatcher.h" namespace GSLAM { class KeyFrameConnection{ public: ORBmatcher *matcherByBoW; ORBmatcher *matcherByTracking; ORBmatcher *matcherByProjection; ORBVocabulary* mpORBVocabulary; KeyFrameDatabase* keyFrameDatabase; void connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2); void connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2,std::vector& matches12); int connectKeyFrame(KeyFrame* keyFrame3,KeyFrame* keyFrame1,KeyFrame* keyFrame2, Transform& transform32,std::vector &matches32,std::vector &matches23); void connectLoop(KeyFrame* keyFrame2); int connectLoop(KeyFrame* keyFrame1,KeyFrame* keyFrame2, Transform& transform12,std::vector &matches12,std::vector &matches21); int connectionThreshold; }; } #endif /* KeyFrameConnection_hpp */ ================================================ FILE: GSLAM/KeyFrameDatabase.cpp ================================================ /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #include "KeyFrameDatabase.h" #include "KeyFrame.h" #include "./DBoW2/BowVector.h" #include using namespace std; namespace GSLAM{ KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc): mpVoc(&voc){ mvInvertedFile.resize(voc.size()); } void KeyFrameDatabase::add(KeyFrame *pKF){ unique_lock lock(mMutex); for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++) mvInvertedFile[vit->first].push_back(pKF); } void KeyFrameDatabase::erase(KeyFrame* pKF){ unique_lock lock(mMutex); // Erase elements in the Inverse File for the entry for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++) { // List of keyframes that share the word list &lKFs = mvInvertedFile[vit->first]; for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { if(pKF==*lit) { lKFs.erase(lit); break; } } } } void KeyFrameDatabase::clear(){ mvInvertedFile.clear(); mvInvertedFile.resize(mpVoc->size()); } vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore){ set spConnectedKeyFrames = pKF->GetConnectedKeyFrames(); list lKFsSharingWords; // Search all keyframes that share a word with current keyframes // Discard keyframes connected to the query keyframe { unique_lock lock(mMutex); for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++) { list &lKFs = mvInvertedFile[vit->first]; for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { KeyFrame* pKFi=*lit; if(pKFi->mnLoopQuery!=pKF->mnId) { pKFi->mnLoopWords=0; if(!spConnectedKeyFrames.count(pKFi)) { pKFi->mnLoopQuery=pKF->mnId; lKFsSharingWords.push_back(pKFi); } } pKFi->mnLoopWords++; } } } if(lKFsSharingWords.empty()) return vector(); list > lScoreAndMatch; // Only compare against those keyframes that share enough words int maxCommonWords=0; for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { if((*lit)->mnLoopWords>maxCommonWords) maxCommonWords=(*lit)->mnLoopWords; } int minCommonWords = maxCommonWords*0.8f; int nscores=0; // Compute similarity score. Retain the matches whose score is higher than minScore for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { KeyFrame* pKFi = *lit; if(pKFi->mnLoopWords>minCommonWords) { nscores++; float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec); pKFi->mLoopScore = si; if(si>=minScore) lScoreAndMatch.push_back(make_pair(si,pKFi)); } } if(lScoreAndMatch.empty()) return vector(); list > lAccScoreAndMatch; float bestAccScore = minScore; // Lets now accumulate score by covisibility for(list >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) { KeyFrame* pKFi = it->second; vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); float bestScore = it->first; float accScore = it->first; KeyFrame* pBestKF = pKFi; for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) { KeyFrame* pKF2 = *vit; if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords) { accScore+=pKF2->mLoopScore; if(pKF2->mLoopScore>bestScore) { pBestKF=pKF2; bestScore = pKF2->mLoopScore; } } } lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF)); if(accScore>bestAccScore) bestAccScore=accScore; } // Return all those keyframes with a score higher than 0.75*bestScore float minScoreToRetain = 0.75f*bestAccScore; set spAlreadyAddedKF; vector vpLoopCandidates; vpLoopCandidates.reserve(lAccScoreAndMatch.size()); for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) { if(it->first>minScoreToRetain) { KeyFrame* pKFi = it->second; if(!spAlreadyAddedKF.count(pKFi)) { vpLoopCandidates.push_back(pKFi); spAlreadyAddedKF.insert(pKFi); } } } return vpLoopCandidates; } vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F){ list lKFsSharingWords; // Search all keyframes that share a word with current frame { unique_lock lock(mMutex); for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++) { list &lKFs = mvInvertedFile[vit->first]; for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { KeyFrame* pKFi=*lit; if(pKFi->mnRelocQuery!=F->mnId) { pKFi->mnRelocWords=0; pKFi->mnRelocQuery=F->mnId; lKFsSharingWords.push_back(pKFi); } pKFi->mnRelocWords++; } } } if(lKFsSharingWords.empty()) return vector(); // Only compare against those keyframes that share enough words int maxCommonWords=0; for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { if((*lit)->mnRelocWords>maxCommonWords) maxCommonWords=(*lit)->mnRelocWords; } int minCommonWords = maxCommonWords*0.8f; list > lScoreAndMatch; int nscores=0; // Compute similarity score. for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { KeyFrame* pKFi = *lit; if(pKFi->mnRelocWords>minCommonWords) { nscores++; float si = mpVoc->score(F->mBowVec,pKFi->mBowVec); pKFi->mRelocScore=si; lScoreAndMatch.push_back(make_pair(si,pKFi)); } } if(lScoreAndMatch.empty()) return vector(); list > lAccScoreAndMatch; float bestAccScore = 0; // Lets now accumulate score by covisibility for(list >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) { KeyFrame* pKFi = it->second; vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); float bestScore = it->first; float accScore = bestScore; KeyFrame* pBestKF = pKFi; for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) { KeyFrame* pKF2 = *vit; if(pKF2->mnRelocQuery!=F->mnId) continue; accScore+=pKF2->mRelocScore; if(pKF2->mRelocScore>bestScore) { pBestKF=pKF2; bestScore = pKF2->mRelocScore; } } lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF)); if(accScore>bestAccScore) bestAccScore=accScore; } // Return all those keyframes with a score higher than 0.75*bestScore float minScoreToRetain = 0.75f*bestAccScore; set spAlreadyAddedKF; vector vpRelocCandidates; vpRelocCandidates.reserve(lAccScoreAndMatch.size()); for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) { const float &si = it->first; if(si>minScoreToRetain) { KeyFrame* pKFi = it->second; if(!spAlreadyAddedKF.count(pKFi)) { vpRelocCandidates.push_back(pKFi); spAlreadyAddedKF.insert(pKFi); } } } return vpRelocCandidates; } } ================================================ FILE: GSLAM/KeyFrameDatabase.h ================================================ /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #ifndef KEYFRAMEDATABASE_H #define KEYFRAMEDATABASE_H #include #include #include #include "KeyFrame.h" #include "Frame.h" #include "ORBVocabulary.h" #include namespace GSLAM { class KeyFrame; class Frame; class KeyFrameDatabase { public: KeyFrameDatabase(const ORBVocabulary &voc); void add(KeyFrame* pKF); void erase(KeyFrame* pKF); void clear(); // Loop Detection std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); // Relocalization std::vector DetectRelocalizationCandidates(Frame* F); protected: // Associated vocabulary const ORBVocabulary* mpVoc; // Inverted file std::vector > mvInvertedFile; // Mutex std::mutex mMutex; }; } //namespace ORB_SLAM #endif ================================================ FILE: GSLAM/L1Solver.h ================================================ // // L1Solver.h // GSLAM // // Created by ctang on 10/2/16. // Copyright © 2016 ctang. All rights reserved. // #ifndef L1Solver_h #define L1Solver_h #include "./coin/ClpSimplex.hpp" #include "./coin/ClpPresolve.hpp" #include "./coin/ClpPrimalColumnSteepest.hpp" #include "./coin/ClpDualRowSteepest.hpp" #endif /* L1Solver_h */ ================================================ FILE: GSLAM/LK.hpp ================================================ // // trackFeatures.hpp // STARCK // // Created by Chaos on 4/1/16. // Copyright © 2016 Chaos. All rights reserved. // #include "KLT.h" #include "ImageGrids.hpp" #include "error.h" #include #include #include "tbb/tbb.h" #include "Homography.hpp" #include #include "opencv2/imgproc/imgproc.hpp" const bool compensate_motion=false; const bool compensate_lighting=false; static float lighting_alpha; static float lighting_beta; cv::Mat homography; int KLTCountRemainingFeatures(KLT_FeatureList fl){ int count = 0; int i; for (i = 0 ; i < fl->nFeatures ; i++){ if (fl->feature[i]->val >= 0){ count++; } } return count; } const int simd_step=8; static void compensatePatchLighting(float* patch, const int width,const int height, const int alignedPatchSize,const float beta){ __m256 _beta = _mm256_set1_ps(beta); for(int p=0;p(yt+h,xt-hw),width*sizeof(float)); } return; } __m256 weight00 = _mm256_set1_ps(weights[0]); __m256 weight01 = _mm256_set1_ps(weights[1]); __m256 weight10 = _mm256_set1_ps(weights[2]); __m256 weight11 = _mm256_set1_ps(weights[3]); for (int h=-hh,y=0;h<=hh;h++,y++) { float *imagePtr0=(float*)image.ptr(h+yt); float *imagePtr1=(float*)image.ptr(h+yt+1); for (int w=-hw,x=0;w<=hw;w+=simd_step,x+=simd_step) { __m256 patch00=_mm256_mul_ps(_mm256_load_ps(&imagePtr0[xt+w]),weight00); __m256 patch01=_mm256_mul_ps(_mm256_load_ps(&imagePtr0[xt+w+1]),weight01); __m256 patch10=_mm256_mul_ps(_mm256_load_ps(&imagePtr1[xt+w]),weight10); __m256 patch11=_mm256_mul_ps(_mm256_load_ps(&imagePtr1[xt+w+1]),weight11); __m256 newpatch=_mm256_add_ps(_mm256_add_ps(patch00,patch01),_mm256_add_ps(patch10,patch11)); _mm256_store_ps(&patch[y*width+x],newpatch); } } memset(&patch[width*height],0,(alignedPatchSize-width*height)*sizeof(float)); } static void computeImageDiff(const float* patch1,const float* patch2,float* diff,const int patchsize){ for (int i=0;i ncols-1-borderx || y < bordery || y > nrows-1-bordery ); } static float computeABSImageDiff(float *patchdiff,const int patchsize){ __m256 a = _mm256_set1_ps(-0.0); __m256 absSum=_mm256_setzero_ps(); for (int i=0;idata + (img->ncols*yt) + xt; assert (xt >= 0 && yt >= 0 && xt <= img->ncols - 2 && yt <= img->nrows - 2); //printf("coef %f %f\n",ax,ay); return ( (1-ax) * (1-ay) * *ptr + ax * (1-ay) * *(ptr+1) + (1-ax) * ay * *(ptr+(img->ncols)) + ax * ay * *(ptr+(img->ncols)+1) ); } /********************************************************************* * _computeIntensityDifference * * Given two images and the window center in both images, * aligns the images wrt the window and computes the difference * between the two overlaid images. */ static void _computeIntensityDifference( _KLT_FloatImage img1, /* images */ _KLT_FloatImage img2, float x1, float y1, /* center of window in 1st img */ float x2, float y2, /* center of window in 2nd img */ int width, int height, /* size of window */ _FloatWindow imgdiff) /* output */ { register int hw = width/2, hh = height/2; float g1, g2; register int i, j; /* Compute values */ for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, img1); g2 = _interpolate(x2+i, y2+j, img2); *imgdiff++ = g1 - g2; } } /********************************************************************* * _computeGradientSum * * Given two gradients and the window center in both images, * aligns the gradients wrt the window and computes the sum of the two * overlaid gradients. */ static void _computeGradientSum( _KLT_FloatImage gradx1, /* gradient images */ _KLT_FloatImage grady1, _KLT_FloatImage gradx2, _KLT_FloatImage grady2, float x1, float y1, /* center of window in 1st img */ float x2, float y2, /* center of window in 2nd img */ int width, int height, /* size of window */ _FloatWindow gradx, /* output */ _FloatWindow grady) /* " */ { register int hw = width/2, hh = height/2; float g1, g2; register int i, j; /* Compute values */ for (j = -hh ; j <= hh ; j++) for (i = -hw ; i <= hw ; i++) { g1 = _interpolate(x1+i, y1+j, gradx1); g2 = _interpolate(x2+i, y2+j, gradx2); *gradx++ = g1 + g2; g1 = _interpolate(x1+i, y1+j, grady1); g2 = _interpolate(x2+i, y2+j, grady2); *grady++ = g1 + g2; } } /********************************************************************* * _compute2by2GradientMatrix * */ static void _compute2by2GradientMatrix( _FloatWindow gradx, _FloatWindow grady, int width, /* size of window */ int height, float *gxx, /* return values */ float *gxy, float *gyy) { register float gx, gy; register int i; /* Compute values */ *gxx = 0.0; *gxy = 0.0; *gyy = 0.0; for (i = 0 ; i < width * height ; i++) { gx = *gradx++; gy = *grady++; *gxx += gx*gx; *gxy += gx*gy; *gyy += gy*gy; } } /********************************************************************* * _compute2by1ErrorVector * */ static void _compute2by1ErrorVector( _FloatWindow imgdiff, _FloatWindow gradx, _FloatWindow grady, int width, /* size of window */ int height, float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ float *ex, /* return values */ float *ey) { register float diff; register int i; /* Compute values */ *ex = 0; *ey = 0; for (i = 0 ; i < width * height ; i++) { diff = *imgdiff++; *ex += diff * (*gradx++); *ey += diff * (*grady++); } *ex *= step_factor; *ey *= step_factor; } /********************************************************************* * _allocateFloatWindow */ static _FloatWindow _allocateFloatWindow( int width, int height) { _FloatWindow fw; fw = (_FloatWindow) malloc(width*height*sizeof(float)); if (fw == NULL) KLTError("(_allocateFloatWindow) Out of memory."); return fw; } static float _sumAbsFloatWindow( _FloatWindow fw, int width, int height) { float sum = 0.0; int w; for ( ; height > 0 ; height--) for (w=0 ; w < width ; w++) sum += (float) fabs(*fw++); return sum; } static int _trackFeature(float x1, /* location of window in first image */ float y1, float *x2, /* starting location of search in second image */ float *y2, cv::Mat &img1, cv::Mat &gradx1, cv::Mat &grady1, cv::Mat &img2, cv::Mat &gradx2, cv::Mat &grady2, int width, /* size of window */ int height, float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ int max_iterations, float small, /* determinant threshold for declaring KLT_SMALL_DET */ float th, /* displacement threshold for stopping */ float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ int lighting_insensitive) /* whether to normalize for gain and bias */ { float gxx, gxy, gyy, ex, ey, dx, dy; int iteration = 0; int status; int hw = width/2; int hh = height/2; int nc = img1.cols; int nr = img1.rows; float one_plus_eps = 1.001f; /* To prevent rounding errors */ float weights1[4],weights2[4]; #ifdef SSE_TRACKING int alignedPatchSize=simd_step*((width*height+simd_step)/simd_step); float *buffers=(float*)malloc(9*alignedPatchSize*sizeof(float)); memset(buffers,0,9*alignedPatchSize*sizeof(float)); float *patch1=buffers; float *gx1=&patch1[alignedPatchSize]; float *gy1=&gx1[alignedPatchSize]; float *patch2=&gy1[alignedPatchSize]; float *gx2=&patch2[alignedPatchSize]; float *gy2=&gx2[alignedPatchSize]; float *patchdiff=&gy2[alignedPatchSize]; float *gx=&patchdiff[alignedPatchSize]; float *gy=&gx[alignedPatchSize]; int xt1,yt1; computeBilinearWeight(x1,y1,xt1,yt1,weights1); if(compensate_lighting){ for(int i=0;i<4;i++) weights1[i]*=lighting_alpha; } computeBilinearPatch(patch1,img1,xt1,yt1,width,height,alignedPatchSize,weights1); computeBilinearPatch(gx1,gradx1,xt1,yt1,width,height,alignedPatchSize,weights1); computeBilinearPatch(gy1,grady1,xt1,yt1,width,height,alignedPatchSize,weights1); if(compensate_lighting){ //printf("aligned size %d\n",alignedPatchSize); /*float _patch[alignedPatchSize]; memset(&_patch[width*height],0,(alignedPatchSize-width*height)*sizeof(float)); for(int i=0;idata=(float*)img1.data; kltimage1->ncols=img1.cols; kltimage1->nrows=img1.rows; kltimage2->data=(float*)img2.data; kltimage2->ncols=img2.cols; kltimage2->nrows=img2.rows; kltgradx1->data=(float*)gradx1.data; kltgradx1->ncols=gradx1.cols; kltgradx1->nrows=gradx1.rows; kltgradx2->data=(float*)gradx2.data; kltgradx2->ncols=gradx2.cols; kltgradx2->nrows=gradx2.rows; kltgrady1->data=(float*)grady1.data; kltgrady1->ncols=grady1.cols; kltgrady1->nrows=grady1.rows; kltgrady2->data=(float*)grady2.data; kltgrady2->ncols=grady2.cols; kltgrady2->nrows=grady2.rows; #endif //printf("%f %f\n",x1,y1); /* Iteratively update the window position */ do { /* If out of bounds, exit loop */ if ( x1-hw < 0.0f || nc-( x1+hw) < one_plus_eps || *x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps || y1-hh < 0.0f || nr-( y1+hh) < one_plus_eps || *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) { status = KLT_OOB; break; } #ifdef SSE_TRACKING int xt2,yt2; computeBilinearWeight(*x2,*y2,xt2,yt2,weights2); computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2); computeBilinearPatch(gx2,gradx2,xt2,yt2,width,height,alignedPatchSize,weights2); computeBilinearPatch(gy2,grady2,xt2,yt2,width,height,alignedPatchSize,weights2); computeImageDiff(patch1,patch2,patchdiff,width*height); computeImageSum(gx1,gx2,gx,width*height); computeImageSum(gy1,gy2,gy,width*height); compute2X2GradientMatrix(gx,gy,width*height,gxx,gyy,gxy); compute2X1ErrorVector(patchdiff,gx,gy,alignedPatchSize,step_factor,ex,ey); #else _computeIntensityDifference(kltimage1,kltimage2, x1, y1, *x2, *y2, width, height, imgdiff); _computeGradientSum(kltgradx1,kltgrady1,kltgradx2,kltgrady2, x1, y1, *x2, *y2, width, height, gradx, grady); _compute2by2GradientMatrix(gradx, grady, width, height, &gxx, &gxy, &gyy); _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor, &ex, &ey); #endif /* Using matrices, solve equation for new displacement */ status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); if (status == KLT_SMALL_DET){ break; } *x2 += dx; *y2 += dy; iteration++; //printf("%f %f\n",dx,dy); } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations); //printf("max iter %d\n",max_iterations); //getchar(); /* Check whether window is out of bounds */ if (*x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps || *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps){ status = KLT_OOB; } /* Check whether residue is too large */ if (status == KLT_TRACKED) { #ifdef SSE_TRACKING int xt2,yt2; computeBilinearWeight(*x2,*y2,xt2,yt2,weights2); computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2); computeImageDiff(patch1,patch2,patchdiff,alignedPatchSize); if (computeABSImageDiff(patchdiff,alignedPatchSize)/(width*height)> max_residue){ status = KLT_LARGE_RESIDUE; } #else _computeIntensityDifference(kltimage1,kltimage2, x1, y1, *x2, *y2, width, height, imgdiff); float diff2=_sumAbsFloatWindow(imgdiff, width, height)/(width*height); if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue) status = KLT_LARGE_RESIDUE; #endif } #ifdef SSE_TRACKING free(buffers); #else free(imgdiff); free(gradx); free(grady); #endif /* Return appropriate value */ if (status == KLT_SMALL_DET){ return KLT_SMALL_DET; } else if (status == KLT_OOB){ return KLT_OOB; } else if (status == KLT_LARGE_RESIDUE){ return KLT_LARGE_RESIDUE; } else if (iteration >= max_iterations) { return KLT_MAX_ITERATIONS; } else return KLT_TRACKED; } static int refineFeatureTranslation(float *x2, float *y2, cv::Mat img1, cv::Mat gradx1, cv::Mat grady1, cv::Mat img2, cv::Mat gradx2, cv::Mat grady2, int width, /* size of window */ int height, float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */ int max_iterations, float small, /* determinant threshold for declaring KLT_SMALL_DET */ float th, float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */ float mdd) /* used affine mapping */ { float gxx, gxy, gyy, ex, ey, dx, dy; int iteration = 0; int status = 0; int hw = width/2; int hh = height/2; int nc2 = img2.cols; int nr2 = img2.rows; float one_plus_eps = 1.001f; /* To prevent rounding errors */ float old_x2 = *x2; float old_y2 = *y2; int alignedPatchSize=simd_step*((width*height+simd_step)/simd_step); float *buffers=(float*)malloc(9*alignedPatchSize*sizeof(float)); memset(buffers,0,9*alignedPatchSize*sizeof(float)); float *patch1=buffers; float *gx1=&patch1[alignedPatchSize]; float *gy1=&gx1[alignedPatchSize]; float *patch2=&gy1[alignedPatchSize]; float *gx2=&patch2[alignedPatchSize]; float *gy2=&gx2[alignedPatchSize]; float *patchdiff=&gy2[alignedPatchSize]; float *gx=&patchdiff[alignedPatchSize]; float *gy=&gx[alignedPatchSize]; memcpy(patch1,img1.data,img1.cols*img1.rows*sizeof(float)); memcpy(gx1,gradx1.data,gradx1.cols*gradx1.rows*sizeof(float)); memcpy(gy1,grady1.data,grady1.cols*grady1.rows*sizeof(float)); float weights2[4]; /* Iteratively update the window position */ do { if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) { status = KLT_OOB; break; } int xt2,yt2; computeBilinearWeight(*x2,*y2,xt2,yt2,weights2); computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2); computeBilinearPatch(gx2,gradx2,xt2,yt2,width,height,alignedPatchSize,weights2); computeBilinearPatch(gy2,grady2,xt2,yt2,width,height,alignedPatchSize,weights2); computeImageDiff(patch1,patch2,patchdiff,width*height); computeImageSum(gx1,gx2,gx,width*height); computeImageSum(gy1,gy2,gy,width*height); compute2X2GradientMatrix(gx,gy,alignedPatchSize,gxx,gyy,gxy); compute2X1ErrorVector(patchdiff,gx,gy,alignedPatchSize,step_factor,ex,ey); status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy); if (status == KLT_SMALL_DET){ break; } *x2 += dx; *y2 += dy; iteration++; } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations); /* Check whether window is out of bounds */ if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps || *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) status = KLT_OOB; /* Check whether feature point has moved to much during iteration*/ if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd ) status = KLT_OOB; /* Check whether residue is too large */ if (status == KLT_TRACKED) { int xt2,yt2; computeBilinearWeight(*x2,*y2,xt2,yt2,weights2); computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2); computeImageDiff(patch1,patch2,patchdiff,alignedPatchSize); //printf("residual %f\n",computeABSImageDiff(patchdiff,alignedPatchSize)/(width*height)); if (computeABSImageDiff(patchdiff,alignedPatchSize)/(width*height)> max_residue){ status = KLT_LARGE_RESIDUE; } } free(buffers); return status; } class KLTInvoker{ private: std::vector* pyramid1; std::vector* pyramid2; KLT_TrackingContext tc; KLT_FeatureList featurelist; float subsampling; public: KLTInvoker(std::vector* _pyramid1, std::vector* _pyramid2, KLT_TrackingContext _tc, KLT_FeatureList _featurelist, float _subsampling){ pyramid1=_pyramid1; pyramid2=_pyramid2; tc=_tc; featurelist=_featurelist; subsampling=_subsampling; }; void operator ()(const tbb::blocked_range& range) const; }; void KLTInvoker::operator()(const tbb::blocked_range& range) const{ for (int indx = range.begin() ; indx < range.end() ; indx++) { /* Only track features that are not lost */ if (featurelist->feature[indx]->val >= 0) { float xloc = featurelist->feature[indx]->x; float yloc = featurelist->feature[indx]->y; /* Transform location to coarsest resolution */ for (int r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { xloc /= subsampling; yloc /= subsampling; } float xlocout,ylocout; if(compensate_motion){ cv::Mat ptMat(3,1,CV_64FC1); ptMat.at(0)=subsampling*xloc; ptMat.at(1)=subsampling*yloc; ptMat.at(2)=1.0; ptMat=homography*ptMat; xlocout = (ptMat.at(0)/ptMat.at(2))/subsampling; ylocout = (ptMat.at(1)/ptMat.at(2))/subsampling; }else{ xlocout =xloc; ylocout =yloc; } int val; /* Beginning with coarsest resolution, do ... */ for (int r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) { //printf("%d %d\n",indx,r); /* Track feature at current resolution */ xloc *= subsampling; yloc *= subsampling; xlocout *= subsampling; ylocout *= subsampling; val = _trackFeature(xloc, yloc, &xlocout, &ylocout, (*pyramid1)[3*r], (*pyramid1)[3*r+1],(*pyramid1)[3*r+2], (*pyramid2)[3*r], (*pyramid2)[3*r+1],(*pyramid2)[3*r+2], tc->window_width, tc->window_height, tc->step_factor, tc->max_iterations, tc->min_determinant, tc->min_displacement, tc->max_residue, tc->lighting_insensitive); if (val==KLT_SMALL_DET || val==KLT_OOB) break; } int ncols=(*pyramid1)[0].cols,nrows=(*pyramid1)[0].rows; /* Record feature */ if (val == KLT_OOB) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_OOB; } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_OOB; } else if (val == KLT_SMALL_DET) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_SMALL_DET; } else if (val == KLT_LARGE_RESIDUE) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_LARGE_RESIDUE; } else if (val == KLT_MAX_ITERATIONS) { featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->val = KLT_MAX_ITERATIONS; } else { featurelist->feature[indx]->x = xlocout; featurelist->feature[indx]->y = ylocout; featurelist->feature[indx]->val = KLT_TRACKED; /*if(featurelist->feature[indx]->aff_x==-1.0||featurelist->feature[indx]->aff_y==-1.0){ if(featurelist->feature[indx]->aff_img==NULL){ featurelist->feature[indx]->aff_img=new cv::Mat(tc->affine_window_height,tc->affine_window_width,CV_32F); featurelist->feature[indx]->aff_gradx=new cv::Mat(tc->affine_window_height,tc->affine_window_width,CV_32F); featurelist->feature[indx]->aff_grady=new cv::Mat(tc->affine_window_height,tc->affine_window_width,CV_32F); } cv::Mat((*pyramid1)[0],cv::Rect(int(xloc-tc->affine_window_width/2), int(yloc-tc->affine_window_height/2), tc->affine_window_width,tc->affine_window_height)).copyTo(*featurelist->feature[indx]->aff_img); cv::Mat((*pyramid1)[1],cv::Rect(int(xloc-tc->affine_window_width/2), int(yloc-tc->affine_window_height/2), tc->affine_window_width,tc->affine_window_height)).copyTo(*featurelist->feature[indx]->aff_gradx); cv::Mat((*pyramid1)[2],cv::Rect(int(xloc-tc->affine_window_width/2), int(yloc-tc->affine_window_height/2), tc->affine_window_width,tc->affine_window_height)).copyTo(*featurelist->feature[indx]->aff_grady); featurelist->feature[indx]->aff_x=featurelist->feature[indx]->aff_y=0.0; }else{ int val=refineFeatureTranslation(&xlocout,&ylocout, *featurelist->feature[indx]->aff_img, *featurelist->feature[indx]->aff_gradx, *featurelist->feature[indx]->aff_grady, (*pyramid2)[0],(*pyramid2)[1],(*pyramid2)[2], tc->affine_window_width,tc->affine_window_height, tc->step_factor,tc->affine_max_iterations,tc->min_determinant, tc->min_displacement,tc->affine_max_residue,tc->affine_max_displacement_differ); featurelist->feature[indx]->val = val; if(val != KLT_TRACKED){ featurelist->feature[indx]->x = -1.0; featurelist->feature[indx]->y = -1.0; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; }else{ } }*/ } } } }; void KLTTrackFeatures(KLT_TrackingContext tc, std::vector &pyramid1, std::vector &pyramid2, KLT_FeatureList featurelist,const Eigen::Matrix3d &invK) { float subsampling = (float) tc->subsampling; int ncols=pyramid1[0].cols,nrows=pyramid1[0].rows; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->affine_window_width % 2 != 1) { tc->affine_window_width = tc->affine_window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->affine_window_width); } if (tc->affine_window_height % 2 != 1) { tc->affine_window_height = tc->affine_window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->affine_window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } tbb::parallel_for(tbb::blocked_range(0,featurelist->nFeatures), KLTInvoker(&pyramid1,&pyramid2,tc,featurelist,subsampling)); std::vector pts1,pts2; for (int i=0;inFeatures;i++) { if(featurelist->feature[i]->val!=KLT_TRACKED){ continue; } Eigen::Vector3d pt((double)featurelist->feature[i]->x,(double)featurelist->feature[i]->y,1.0); pt=invK*pt; featurelist->feature[i]->norm=pt; featurelist->feature[i]->norm.normalize(); featurelist->feature[i]->vec=featurelist->feature[i]->norm; featurelist->feature[i]->vec/=featurelist->feature[i]->vec(2); } featurelist->isOutlierRejected=false; if (KLT_verbose >= 1) { fprintf(stderr, "\n\t%d features successfully tracked.\n", KLTCountRemainingFeatures(featurelist)); if (tc->writeInternalImages) fprintf(stderr, "\tWrote images to 'kltimg_tf*.pgm'.\n"); fflush(stderr); } } #include void compensateLightingAndMotion(const std::vector& pts1, const std::vector& pts2, const std::vector& intensity1, const std::vector& intensity2){ //compensate motion int good_count; std::vector status; homography=GSLAM::EstimateHomography(pts1,pts2,status,good_count); //compensate lighting double error0=0; double error=0; double light_error0=0.0; double light_error=0.0;; int inlier_count=0; Eigen::MatrixXd A=Eigen::MatrixX2d(good_count,2); Eigen::VectorXd b=Eigen::VectorXd(good_count); for(int i=0;ilight_error0){ lighting_alpha=1.0; lighting_beta=0.0; } static std::ofstream record("/Users/chaos/Desktop/debug/lighting.txt"); record< #include namespace GSLAM{ class KeyFrame; class LocalMapPoint{ public: Eigen::Vector3d globalPosition; unsigned char color[3]; unsigned char vcolor[3]; Eigen::Vector3d vec; Eigen::Vector3d norm; double invdepth; std::vector errors; std::vector vecs; std::vector pVectors; std::vector tracked; bool isDeleted; bool isFullTrack; bool isEstimated; int measurementCount; float mfMinDistance; float minLevelScaleFactor; float mfMaxDistance; float maxLevelScaleFactor; Eigen::Vector3d getPosition() const{ return vec/invdepth; } bool isValid(){ return measurementCount>0; } int PredictScale(const float ¤tDist,const float &logScaleFactor){ float ratio= mfMaxDistance/currentDist; return int(std::ceil(std::log(ratio)/logScaleFactor)); } float GetMinDistanceInvariance(){ return 0.8f*mfMinDistance; } float GetMaxDistanceInvariance(){ return 1.2f*mfMaxDistance; } void updateNormalAndDepth(){ const float dist = (float)vec.norm()/invdepth; mfMaxDistance = dist*maxLevelScaleFactor; mfMinDistance = mfMaxDistance/minLevelScaleFactor; } int globalIndex; }; } #endif ================================================ FILE: GSLAM/ORBVocabulary.h ================================================ /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #ifndef ORBVOCABULARY_H #define ORBVOCABULARY_H #include "./DBoW2/FORB.h" #include "./DBoW2/TemplatedVocabulary.h" namespace GSLAM{ typedef DBoW2::TemplatedVocabulary ORBVocabulary; } #endif ================================================ FILE: GSLAM/ORBextractor.cc ================================================ /** * This file is part of ORB-SLAM2. * This file is based on the file orb.cpp from the OpenCV library (see BSD license below). * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ /** * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * */ #include //#include #include #include #include #include "ORBextractor.h" using namespace cv; using namespace std; namespace GSLAM { const int PATCH_SIZE = 31; const int HALF_PATCH_SIZE = 15; //const int EDGE_THRESHOLD = 19; const int EDGE_THRESHOLD = 25; static void HarrisResponses(const Mat& img, vector& pts, int blockSize, float harris_k) { CV_Assert( img.type() == CV_8UC1 && blockSize*blockSize <= 2048 ); size_t ptidx, ptsize = pts.size(); const uchar* ptr00 = img.ptr(); int step = (int)(img.step/img.elemSize1()); int r = blockSize/2; float scale = (1 << 2) * blockSize * 255.0f; scale = 1.0f / scale; float scale_sq_sq = scale * scale * scale * scale; AutoBuffer ofsbuf(blockSize*blockSize); int* ofs = ofsbuf; for( int i = 0; i < blockSize; i++ ) for( int j = 0; j < blockSize; j++ ) ofs[i*blockSize + j] = (int)(i*step + j); for( ptidx = 0; ptidx < ptsize; ptidx++ ) { int x0 = cvRound(pts[ptidx].pt.x - r); int y0 = cvRound(pts[ptidx].pt.y - r); const uchar* ptr0 = ptr00 + y0*step + x0; int a = 0, b = 0, c = 0; for( int k = 0; k < blockSize*blockSize; k++ ) { const uchar* ptr = ptr0 + ofs[k]; int Ix = (ptr[1] - ptr[-1])*2 + (ptr[-step+1] - ptr[-step-1]) + (ptr[step+1] - ptr[step-1]); int Iy = (ptr[step] - ptr[-step])*2 + (ptr[step-1] - ptr[-step-1]) + (ptr[step+1] - ptr[-step+1]); a += Ix*Ix; b += Iy*Iy; c += Ix*Iy; } pts[ptidx].response = ((float)a * b - (float)c * c - harris_k * ((float)a + b) * ((float)a + b))*scale_sq_sq; } } static float IC_Angle(const Mat& image, Point2f pt, const vector & u_max) { int m_01 = 0, m_10 = 0; const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x)); // Treat the center line differently, v=0 for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u) m_10 += u * center[u]; // Go line by line in the circuI853lar patch int step = (int)image.step1(); for (int v = 1; v <= HALF_PATCH_SIZE; ++v) { // Proceed over the two lines int v_sum = 0; int d = u_max[v]; for (int u = -d; u <= d; ++u) { int val_plus = center[u + v*step], val_minus = center[u - v*step]; v_sum += (val_plus - val_minus); m_10 += u * (val_plus + val_minus); } m_01 += v * v_sum; } return fastAtan2((float)m_01, (float)m_10); } const float factorPI = (float)(CV_PI/180.f); static void computeOrbDescriptor(const KeyPoint& kpt, const Mat& img, const Point* pattern, uchar* desc) { float angle = (float)kpt.angle*factorPI; float a = (float)cos(angle), b = (float)sin(angle); const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x)); const int step = (int)img.step; #define GET_VALUE(idx) \ center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \ cvRound(pattern[idx].x*a - pattern[idx].y*b)] for (int i = 0; i < 32; ++i, pattern += 16) { int t0, t1, val; t0 = GET_VALUE(0); t1 = GET_VALUE(1); val = t0 < t1; t0 = GET_VALUE(2); t1 = GET_VALUE(3); val |= (t0 < t1) << 1; t0 = GET_VALUE(4); t1 = GET_VALUE(5); val |= (t0 < t1) << 2; t0 = GET_VALUE(6); t1 = GET_VALUE(7); val |= (t0 < t1) << 3; t0 = GET_VALUE(8); t1 = GET_VALUE(9); val |= (t0 < t1) << 4; t0 = GET_VALUE(10); t1 = GET_VALUE(11); val |= (t0 < t1) << 5; t0 = GET_VALUE(12); t1 = GET_VALUE(13); val |= (t0 < t1) << 6; t0 = GET_VALUE(14); t1 = GET_VALUE(15); val |= (t0 < t1) << 7; desc[i] = (uchar)val; } #undef GET_VALUE } static int bit_pattern_31_[256*4] = { 8,-3, 9,5/*mean (0), correlation (0)*/, 4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/, -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/, 7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/, 2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/, 1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/, -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/, -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/, -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/, 10,4, 11,9/*mean (0.122065), correlation (0.093285)*/, -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/, -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/, 7,7, 12,6/*mean (0.160583), correlation (0.130064)*/, -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/, -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/, -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/, 12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/, -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/, -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/, 11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/, 4,7, 5,1/*mean (0.205106), correlation (0.186848)*/, 5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/, 3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/, -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/, -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/, -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/, -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/, -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/, -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/, 5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/, 5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/, 1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/, 9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/, 4,7, 4,12/*mean (0.131005), correlation (0.257622)*/, 2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/, -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/, -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/, 4,11, 9,12/*mean (0.226226), correlation (0.258255)*/, 0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/, -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/, -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/, -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/, 8,12, 10,7/*mean (0.225337), correlation (0.282851)*/, 0,9, 1,3/*mean (0.226687), correlation (0.278734)*/, 7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/, -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/, 10,7, 12,1/*mean (0.125517), correlation (0.31089)*/, -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/, 10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/, -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/, -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/, 3,3, 7,8/*mean (0.177755), correlation (0.309394)*/, 5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/, -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/, 3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/, 2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/, -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/, -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/, -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/, -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/, 6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/, -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/, -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/, -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/, 3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/, -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/, -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/, 2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/, -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/, -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/, 5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/, -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/, -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/, -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/, 10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/, 7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/, -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/, -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/, 7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/, -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/, -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/, -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/, 7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/, -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/, 1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/, 2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/, -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/, -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/, 7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/, 1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/, 9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/, -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/, -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/, 7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/, 12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/, 6,3, 7,11/*mean (0.1074), correlation (0.413224)*/, 5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/, 2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/, 3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/, 2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/, 9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/, -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/, -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/, 1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/, 6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/, 2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/, 6,3, 11,0/*mean (0.204588), correlation (0.411762)*/, 3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/, 7,8, 9,3/*mean (0.213237), correlation (0.409306)*/, -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/, -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/, -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/, -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/, 8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/, 4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/, -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/, 4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/, -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/, -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/, 7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/, -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/, -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/, 8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/, -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/, 1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/, 7,-4, 9,1/*mean (0.132692), correlation (0.454)*/, -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/, 11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/, -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/, 3,7, 7,12/*mean (0.147627), correlation (0.456643)*/, 5,5, 10,8/*mean (0.152901), correlation (0.455036)*/, 0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/, -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/, 0,7, 2,12/*mean (0.18312), correlation (0.433855)*/, -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/, 5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/, 3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/, -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/, -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/, -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/, 6,5, 8,0/*mean (0.1972), correlation (0.450481)*/, -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/, -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/, 1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/, 4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/, -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/, 2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/, -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/, 4,1, 9,3/*mean (0.23962), correlation (0.444824)*/, -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/, -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/, 7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/, 4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/, -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/, 7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/, 7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/, -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/, -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/, -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/, 2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/, 10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/, -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/, 8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/, 2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/, -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/, -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/, -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/, 5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/, -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/, -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/, -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/, -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/, -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/, 2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/, -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/, -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/, -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/, -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/, 6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/, -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/, 11,11, 12,6/*mean (0.16652), correlation (0.497632)*/, 7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/, -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/, -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/, -7,1, -6,7/*mean (0.175), correlation (0.500024)*/, -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/, -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/, -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/, -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/, -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/, 1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/, 1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/, 9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/, 5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/, -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/, -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/, -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/, -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/, 8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/, 2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/, 7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/, -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/, -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/, 4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/, 3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/, -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/, 5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/, 4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/, -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/, 0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/, -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/, 3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/, -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/, 8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/, -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/, 2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/, 10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/, 6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/, -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/, -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/, -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/, -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/, -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/, 4,2, 12,12/*mean (0.01778), correlation (0.546921)*/, 2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/, 6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/, 3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/, 11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/, -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/, 4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/, 2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/, -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/, -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/, -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/, 6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/, 0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/, -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/, -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/, -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/, 5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/, 2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/, -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/, 9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/, 11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/, 3,0, 3,5/*mean (0.101147), correlation (0.525576)*/, -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/, 3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/, -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/, 5,8, 12,11/*mean (0.114181), correlation (0.555793)*/, 8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/, 7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/, -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/, 7,3, 12,4/*mean (0.122582), correlation (0.555825)*/, 9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/, 7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/, -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/ }; ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, int _iniThFAST, int _minThFAST): nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), iniThFAST(_iniThFAST), minThFAST(_minThFAST) { mvScaleFactor.resize(nlevels); mvLevelSigma2.resize(nlevels); mvScaleFactor[0]=1.0f; mvLevelSigma2[0]=1.0f; for(int i=1; i= vmin; --v) { while (umax[v0] == umax[v0 + 1]) ++v0; umax[v] = v0; ++v0; } } static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax) { for (vector::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){ keypoint->angle = IC_Angle(image, keypoint->pt, umax); } } void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4) { const int halfX = ceil(static_cast(UR.x-UL.x)/2); const int halfY = ceil(static_cast(BR.y-UL.y)/2); //Define boundaries of childs n1.UL = UL; n1.UR = cv::Point2i(UL.x+halfX,UL.y); n1.BL = cv::Point2i(UL.x,UL.y+halfY); n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY); n1.vKeys.reserve(vKeys.size()); n2.UL = n1.UR; n2.UR = UR; n2.BL = n1.BR; n2.BR = cv::Point2i(UR.x,UL.y+halfY); n2.vKeys.reserve(vKeys.size()); n3.UL = n1.BL; n3.UR = n1.BR; n3.BL = BL; n3.BR = cv::Point2i(n1.BR.x,BL.y); n3.vKeys.reserve(vKeys.size()); n4.UL = n3.UR; n4.UR = n2.BR; n4.BL = n3.BR; n4.BR = BR; n4.vKeys.reserve(vKeys.size()); //Associate points to childs for(size_t i=0;i ORBextractor::DistributeOctTree(const vector& vToDistributeKeys, const int &minX, const int &maxX, const int &minY, const int &maxY, const int &N, const int &level) { // Compute how many initial nodes const int nIni = round(static_cast(maxX-minX)/(maxY-minY)); const float hX = static_cast(maxX-minX)/nIni; list lNodes; vector vpIniNodes; vpIniNodes.resize(nIni); for(int i=0; i(i),0); ni.UR = cv::Point2i(hX*static_cast(i+1),0); ni.BL = cv::Point2i(ni.UL.x,maxY-minY); ni.BR = cv::Point2i(ni.UR.x,maxY-minY); ni.vKeys.reserve(vToDistributeKeys.size()); lNodes.push_back(ni); vpIniNodes[i] = &lNodes.back(); } //Associate points to childs for(size_t i=0;ivKeys.push_back(kp); } list::iterator lit = lNodes.begin(); while(lit!=lNodes.end()) { if(lit->vKeys.size()==1) { lit->bNoMore=true; lit++; } else if(lit->vKeys.empty()) lit = lNodes.erase(lit); else lit++; } bool bFinish = false; int iteration = 0; vector > vSizeAndPointerToNode; vSizeAndPointerToNode.reserve(lNodes.size()*4); while(!bFinish) { iteration++; int prevSize = lNodes.size(); lit = lNodes.begin(); int nToExpand = 0; vSizeAndPointerToNode.clear(); while(lit!=lNodes.end()) { if(lit->bNoMore) { // If node only contains one point do not subdivide and continue lit++; continue; } else { // If more than one point, subdivide ExtractorNode n1,n2,n3,n4; lit->DivideNode(n1,n2,n3,n4); // Add childs if they contain points if(n1.vKeys.size()>0) { lNodes.push_front(n1); if(n1.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n2.vKeys.size()>0) { lNodes.push_front(n2); if(n2.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n3.vKeys.size()>0) { lNodes.push_front(n3); if(n3.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n4.vKeys.size()>0) { lNodes.push_front(n4); if(n4.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } lit=lNodes.erase(lit); continue; } } // Finish if there are more nodes than required features // or all nodes contain just one point if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) { bFinish = true; } else if(((int)lNodes.size()+nToExpand*3)>N) { while(!bFinish) { prevSize = lNodes.size(); vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; vSizeAndPointerToNode.clear(); sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end()); for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--) { ExtractorNode n1,n2,n3,n4; vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4); // Add childs if they contain points if(n1.vKeys.size()>0) { lNodes.push_front(n1); if(n1.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n2.vKeys.size()>0) { lNodes.push_front(n2); if(n2.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n3.vKeys.size()>0) { lNodes.push_front(n3); if(n3.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n4.vKeys.size()>0) { lNodes.push_front(n4); if(n4.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit); if((int)lNodes.size()>=N) break; } if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) bFinish = true; } } } // Retain the best point in each node vector vResultKeys; vResultKeys.reserve(nfeatures); for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++) { vector &vNodeKeys = lit->vKeys; cv::KeyPoint* pKP = &vNodeKeys[0]; float maxResponse = pKP->response; for(size_t k=1;kmaxResponse) { pKP = &vNodeKeys[k]; maxResponse = vNodeKeys[k].response; } } vResultKeys.push_back(*pKP); } return vResultKeys; } void ORBextractor::ComputeKeyPointsOctTree(vector >& allKeypoints) { allKeypoints.resize(nlevels); const float W = 30; //Ptr detector=GFTTDetector::create(); for (int level = 0; level < nlevels; ++level) { const int minBorderX = EDGE_THRESHOLD-3; const int minBorderY = minBorderX; const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3; const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3; vector vToDistributeKeys; vToDistributeKeys.reserve(nfeatures*10); const float width = (maxBorderX-minBorderX); const float height = (maxBorderY-minBorderY); const int nCols = width/W; const int nRows = height/W; const int wCell = ceil(width/nCols); const int hCell = ceil(height/nRows); for(int i=0; i=maxBorderY-3) continue; if(maxY>maxBorderY) maxY = maxBorderY; for(int j=0; j=maxBorderX-6) continue; if(maxX>maxBorderX) maxX = maxBorderX; vector vKeysCell; FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,iniThFAST,true); if(vKeysCell.empty()){ FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,minThFAST,true); } /*detector->detect(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell);*/ if(!vKeysCell.empty()) { for(vector::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++) { (*vit).pt.x+=j*wCell; (*vit).pt.y+=i*hCell; vToDistributeKeys.push_back(*vit); } } } } vector & keypoints = allKeypoints[level]; keypoints.reserve(nfeatures); keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY,mnFeaturesPerLevel[level], level); const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; // Add border to coordinates and scale information const int nkps = keypoints.size(); for(int i=0; i > &allKeypoints) { allKeypoints.resize(nlevels); float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows; for (int level = 0; level < nlevels; ++level) { const int nDesiredFeatures = mnFeaturesPerLevel[level]; const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio)); const int levelRows = imageRatio*levelCols; const int minBorderX = EDGE_THRESHOLD; const int minBorderY = minBorderX; const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD; const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD; const int W = maxBorderX - minBorderX; const int H = maxBorderY - minBorderY; const int cellW = ceil((float)W/levelCols); const int cellH = ceil((float)H/levelRows); const int nCells = levelRows*levelCols; const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells); vector > > cellKeyPoints(levelRows, vector >(levelCols)); vector > nToRetain(levelRows,vector(levelCols,0)); vector > nTotal(levelRows,vector(levelCols,0)); vector > bNoMore(levelRows,vector(levelCols,false)); vector iniXCol(levelCols); vector iniYRow(levelRows); int nNoMore = 0; int nToDistribute = 0; float hY = cellH + 6; for(int i=0; infeaturesCell) { nToRetain[i][j] = nfeaturesCell; bNoMore[i][j] = false; } else { nToRetain[i][j] = nKeys; nToDistribute += nfeaturesCell-nKeys; bNoMore[i][j] = true; nNoMore++; } } } // Retain by score while(nToDistribute>0 && nNoMorenNewFeaturesCell) { nToRetain[i][j] = nNewFeaturesCell; bNoMore[i][j] = false; } else { nToRetain[i][j] = nTotal[i][j]; nToDistribute += nNewFeaturesCell-nTotal[i][j]; bNoMore[i][j] = true; nNoMore++; } } } } } vector & keypoints = allKeypoints[level]; keypoints.reserve(nDesiredFeatures*2); const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; // Retain by score and transform coordinates for(int i=0; i &keysCell = cellKeyPoints[i][j]; KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]); if((int)keysCell.size()>nToRetain[i][j]) keysCell.resize(nToRetain[i][j]); for(size_t k=0, kend=keysCell.size(); knDesiredFeatures) { KeyPointsFilter::retainBest(keypoints,nDesiredFeatures); keypoints.resize(nDesiredFeatures); } } // and compute orientations for (int level = 0; level < nlevels; ++level) computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); } void ORBextractor::ComputeKeyPointsEigVal(cv::Mat dx,cv::Mat dy,std::vector >& allKeypoints){ } static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors, const vector& pattern) { descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1); for (size_t i = 0; i < keypoints.size(); i++){ //printf("%d %d %f %f\n",image.cols,image.rows,keypoints[i].pt.x,keypoints[i].pt.y); computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i)); } } void ORBextractor::operator()(InputArray _image, InputArray _mask, vector& _keypoints,OutputArray _descriptors){ if(_image.empty()) return; Mat image = _image.getMat(); assert(image.type() == CV_8UC1 ); // Pre-compute the scale pyramid ComputePyramid(image); vector < vector > allKeypoints; ComputeKeyPointsOctTree(allKeypoints); Mat descriptors; int nkeypoints = 0; for (int level = 0; level < nlevels; ++level) nkeypoints += (int)allKeypoints[level].size(); if( nkeypoints == 0 ) _descriptors.release(); else { _descriptors.create(nkeypoints, 32, CV_8U); descriptors = _descriptors.getMat(); } _keypoints.clear(); _keypoints.reserve(nkeypoints); int offset = 0; for (int level = 0; level < nlevels; ++level) { vector& keypoints = allKeypoints[level]; int nkeypointsLevel = (int)keypoints.size(); if(nkeypointsLevel==0) continue; // preprocess the resized image Mat workingMat = mvImagePyramid[level].clone(); GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101); // Compute the descriptors Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); computeDescriptors(workingMat, keypoints, desc, pattern); offset += nkeypointsLevel; // Scale keypoint coordinates if (level != 0){ float scale = mvScaleFactor[level]; for (vector::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){ keypoint->pt *= scale; } } // And add the keypoints to the output _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end()); } } void ORBextractor::ComputePyramid(cv::Mat image) { for (int level = 0; level < nlevels; ++level) { float scale = mvInvScaleFactor[level]; Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale)); Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2); Mat temp(wholeSize, image.type()), masktemp; mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); // Compute the resized image if( level != 0 ){ resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR); copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101+BORDER_ISOLATED); } else{ copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101); } } } } //namespace ORB_SLAM ================================================ FILE: GSLAM/ORBextractor.h ================================================ /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #ifndef ORBEXTRACTOR_H #define ORBEXTRACTOR_H #include #include //#include #include "opencv2/core/core_c.h" #include "opencv2/imgproc/imgproc_c.h" namespace GSLAM { class ExtractorNode { public: ExtractorNode():bNoMore(false){} void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); std::vector vKeys; cv::Point2i UL, UR, BL, BR; std::list::iterator lit; bool bNoMore; }; class ORBextractor { public: enum {HARRIS_SCORE=0, FAST_SCORE=1 }; ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST, int minThFAST); ~ORBextractor(){} // Compute the ORB features and descriptors on an image. // ORB are dispersed on the image using an octree. // Mask is ignored in the current implementation. void operator()( cv::InputArray image, cv::InputArray mask, std::vector& keypoints, cv::OutputArray descriptors); int inline GetLevels(){ return nlevels; } float inline GetScaleFactor(){ return scaleFactor;} std::vector inline GetScaleFactors(){ return mvScaleFactor; } std::vector inline GetInverseScaleFactors(){ return mvInvScaleFactor; } std::vector inline GetScaleSigmaSquares(){ return mvLevelSigma2; } std::vector inline GetInverseScaleSigmaSquares(){ return mvInvLevelSigma2; } std::vector mvImagePyramid; protected: void ComputePyramid(cv::Mat image); void ComputeKeyPointsOctTree(std::vector >& allKeypoints); std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); void ComputeKeyPointsOld(std::vector >& allKeypoints); void ComputeKeyPointsUniform(std::vector >& allKeypoints); void ComputeKeyPointsEigVal(cv::Mat dx,cv::Mat dy,std::vector >& allKeypoints); std::vector pattern; int nfeatures; double scaleFactor; int nlevels; int iniThFAST; int minThFAST; std::vector mnFeaturesPerLevel; std::vector umax; std::vector mvScaleFactor; std::vector mvInvScaleFactor; std::vector mvLevelSigma2; std::vector mvInvLevelSigma2; }; } //namespace GSLAM #endif ================================================ FILE: GSLAM/ORBmatcher.cc ================================================ /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #include "ORBmatcher.h" #include #include "opencv2/core/core.hpp" #include "opencv2/features2d/features2d.hpp" #include "./DBoW2/FeatureVector.h" #include #include #include "Geometry.h" using namespace std; namespace GSLAM { const int ORBmatcher::TH_HIGH = 100; const int ORBmatcher::TH_LOW = 64; const int ORBmatcher::HISTO_LENGTH = 30; enum{ NOT_MATCHED=-1, NO_MATCH_CANDIDATE=-2, LARGE_MATCH_DISTANCE=-3, OUT_OF_DISTANCE_RANGE=-4, ROTATION_INCONSISTENT=-5 }; ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbCheckOrientation(checkOri){ } bool errorCompare(const pair& e1, const pair& e2) { return e1.second < e2.second; } int ORBmatcher::SearchByRotation(Frame* frame1,Frame* frame2, const cv::Mat& rotation, std::vector& pts1, std::vector& pts2, int ORBdist){ int nMatches=0; std::vector matches12(frame1->mvKeys.size(),-1); std::vector matches21(frame2->mvKeys.size(),-1); pts1.clear(); pts2.clear(); for (int p=0;pmvKeys.size();p++) { if (matches12[p]>=0) { nMatches++; continue; } if (true) { cv::Mat ptMat(3,1,CV_64FC1); ptMat.at(0)=frame1->mvKeys[p].pt.x; ptMat.at(1)=frame1->mvKeys[p].pt.y; ptMat.at(2)=1.0; ptMat=rotation*ptMat; float u=(float)(ptMat.at(0)/ptMat.at(2)), v=(float)(ptMat.at(1)/ptMat.at(2)); if(umnMinX || u>frame2->mnMaxX) continue; if(vmnMinY || v>frame2->mnMaxY) continue; std::vector squareDistances; const vector vIndices2 = frame2->GetFeaturesInArea(u,v,20.0,0,2,squareDistances); if(vIndices2.empty()){ matches12[p]=NO_MATCH_CANDIDATE; continue; } const cv::Mat dMP = frame1->mDescriptors.row(p); int bestDist = 256; int bestIdx2 = -1; for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){ const size_t i2 = *vit; if(matches21[i2]>=0){ continue; } const cv::Mat &d = frame2->mDescriptors.row(i2); const int dist = DescriptorDistance(dMP,d); if(distmvKeys[p].pt); pts2.push_back(frame2->mvKeys[bestIdx2].pt); nMatches++; }else{ matches12[p]=LARGE_MATCH_DISTANCE; } } } return nMatches; } int ORBmatcher::SearchByTracking(KeyFrame *keyFrame1,KeyFrame *keyFrame2, vector &matches12,vector &matches21, const int ORBdist){ std::vector > errorIndex; for (int p=0;pmvLocalMapPoints.size();p++) { if (keyFrame1->mvLocalMapPoints[p].isEstimated &&keyFrame1->mvLocalMapPoints[p].isValid() &&keyFrame1->mvLocalMapPoints[p].vecs.back()!=NULL) { errorIndex.push_back(make_pair(p,keyFrame1->mvLocalMapPoints[p].errors.back()*K(0,0))); } } std::sort(errorIndex.begin(),errorIndex.end(),errorCompare); int nMatches=0; for (int i=0;imvLocalMapPoints[p].vecs.back()); project/=project(2); const int nPredictedLevel=keyFrame1->framePtr->mvKeysUn[p].octave; float radius=3*std::max(error,2.f); vector squareDistances; const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(project(0),project(1), radius,nPredictedLevel-2, nPredictedLevel+2, squareDistances); if (vIndices2.empty()) { matches12[p]=NO_MATCH_CANDIDATE; continue; } const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p); int bestDist = 256; int bestIdx2 = -1; for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end();vit++){ const size_t i2 = *vit; if(matches21[i2]>=0){ continue; } const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2); const int dist = DescriptorDistance(dMP,d); if(dist &matches12,vector &matches21,const float th,const int ORBdist){ int nMatches=0; // Rotation Histogram (to check rotation consistency) vector rotHist[HISTO_LENGTH]; for(int i=0;imvLocalMapPoints.size();p++) { if (matches12[p]>=0) { nMatches++; continue; } if (keyFrame1->mvLocalMapPoints[p].isEstimated) { Eigen::Vector3d position1=keyFrame1->mvLocalMapPoints[p].getPosition(); Eigen::Vector3d position2=transform.rotation*(keyFrame1->mvLocalMapPoints[p].getPosition()-transform.translation); Eigen::Vector3d predict; predict=K*position2; predict/=predict(2); float u=(float)predict(0),v=(float)predict(1); if(uframePtr->mnMinX || u>keyFrame2->framePtr->mnMaxX) continue; if(vframePtr->mnMinY || v>keyFrame2->framePtr->mnMaxY) continue; Eigen::Vector3d PO= position1-transform.translation; double dist3D=PO.norm(); const float maxDistance=keyFrame1->mvLocalMapPoints[p].GetMaxDistanceInvariance(); const float minDistance=keyFrame1->mvLocalMapPoints[p].GetMinDistanceInvariance(); if(dist3DmaxDistance){ matches12[p]=OUT_OF_DISTANCE_RANGE; continue; } int nPredictedLevel =keyFrame1->mvLocalMapPoints[p].PredictScale(dist3D,keyFrame2->framePtr->mfLogScaleFactor); nPredictedLevel=nPredictedLevel<0?0:nPredictedLevel; nPredictedLevel=nPredictedLevel>=keyFrame1->framePtr->mnScaleLevels? keyFrame1->framePtr->mnScaleLevels-1:nPredictedLevel; const float radius = th*keyFrame2->framePtr->mvScaleFactors[nPredictedLevel]; std::vector squareDistances; const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(u,v,radius, nPredictedLevel-2,nPredictedLevel+2, squareDistances); if(vIndices2.empty()){ matches12[p]=NO_MATCH_CANDIDATE; continue; } const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p); int bestDist = 256; int bestIdx2 = -1; for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){ const size_t i2 = *vit; if(matches21[i2]>=0){ continue; } const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2); const int dist = DescriptorDistance(dMP,d); if(distframePtr->mvKeysUn[p].angle-keyFrame1->framePtr->mvKeysUn[bestIdx2].angle; if(rot<0.0) rot+=360.0f; int bin = round(rot*factor); if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin &matches12,vector &matches21, const float th,const int ORBdist){ int nMatches=0; // Rotation Histogram (to check rotation consistency) vector rotHist[HISTO_LENGTH]; for(int i=0;imvLocalMapPoints.size();p++) { if (matches12[p]>=0) { assert(matches21[matches12[p]]==p); nMatches++; continue; } if (keyFrame1->mvLocalMapPoints[p].isEstimated) { Eigen::Vector3d position1=keyFrame1->mvLocalMapPoints[p].getPosition(); Eigen::Vector3d position2=transform.rotation*(keyFrame1->mvLocalMapPoints[p].getPosition()-transform.translation); Eigen::Vector3d predict; predict=K*position2; predict/=predict(2); float u=(float)predict(0),v=(float)predict(1); if(uframePtr->mnMinX || u>keyFrame2->framePtr->mnMaxX) continue; if(vframePtr->mnMinY || v>keyFrame2->framePtr->mnMaxY) continue; Eigen::Vector3d PO= position1-transform.translation; double dist3D=PO.norm(); const float maxDistance=keyFrame1->mvLocalMapPoints[p].GetMaxDistanceInvariance(); const float minDistance=keyFrame1->mvLocalMapPoints[p].GetMinDistanceInvariance(); if(dist3DmaxDistance){ matches12[p]=OUT_OF_DISTANCE_RANGE; continue; } int nPredictedLevel =keyFrame1->mvLocalMapPoints[p].PredictScale(dist3D,keyFrame2->framePtr->mfLogScaleFactor); nPredictedLevel=nPredictedLevel<0?0:nPredictedLevel; nPredictedLevel=nPredictedLevel>=keyFrame1->framePtr->mnScaleLevels? keyFrame1->framePtr->mnScaleLevels-1:nPredictedLevel; const float radius = th*keyFrame2->framePtr->mvScaleFactors[nPredictedLevel]; std::vector squareDistances; const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(u,v,radius, nPredictedLevel-2,nPredictedLevel+2, squareDistances); if(vIndices2.empty()){ matches12[p]=NO_MATCH_CANDIDATE; continue; } const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p); int bestDist = 256; int bestIdx2 = -1; for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){ const size_t i2 = *vit; if(matches21[i2]>=0){ continue; } const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2); const int dist = DescriptorDistance(dMP,d); if(distframePtr->mvKeysUn[p].angle-keyFrame1->framePtr->mvKeysUn[bestIdx2].angle; if(rot<0.0) rot+=360.0f; int bin = round(rot*factor); if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin &matches12,vector &matches21, const float th,const int ORBdist){ int nMatches=0; vector rotHist[HISTO_LENGTH]; for(int i=0;imvLocalMapPoints.size();p++) { if (matches12[p]>=0) { assert(matches21[matches12[p]]==p); nMatches++; continue; } if (keyFrame1->mvLocalMapPoints[p].isEstimated) { Eigen::Vector3d position1=keyFrame1->mvLocalMapPoints[p].getPosition(); Eigen::Vector3d position2=transform.rotation*(keyFrame1->mvLocalMapPoints[p].getPosition()-transform.translation); Eigen::Vector3d predict; predict=K*position2; predict/=predict(2); float u=(float)predict(0),v=(float)predict(1); if(uframePtr->mnMinX || u>keyFrame2->framePtr->mnMaxX) continue; if(vframePtr->mnMinY || v>keyFrame2->framePtr->mnMaxY) continue; Eigen::Vector3d PO= position1-transform.translation; double dist3D=PO.norm(); const float maxDistance=keyFrame1->mvLocalMapPoints[p].GetMaxDistanceInvariance(); const float minDistance=keyFrame1->mvLocalMapPoints[p].GetMinDistanceInvariance(); if(dist3DmaxDistance){ matches12[p]=OUT_OF_DISTANCE_RANGE; continue; } int nPredictedLevel =keyFrame1->mvLocalMapPoints[p].PredictScale(dist3D,keyFrame2->framePtr->mfLogScaleFactor); nPredictedLevel=nPredictedLevel<0?0:nPredictedLevel; nPredictedLevel=nPredictedLevel>=keyFrame1->framePtr->mnScaleLevels? keyFrame1->framePtr->mnScaleLevels-1:nPredictedLevel; const float radius = th*keyFrame2->framePtr->mvScaleFactors[nPredictedLevel]; std::vector squareDistances; const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(u,v,radius, nPredictedLevel-2, nPredictedLevel+2, squareDistances); if(vIndices2.empty()){ matches12[p]=NO_MATCH_CANDIDATE; continue; } const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p); int bestDist = 256; int bestIdx2 = -1; for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){ const size_t i2 = *vit; if(matches21[i2]>=0||!keyFrame2->mvLocalMapPoints[i2].isEstimated){ continue; } const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2); const int dist = DescriptorDistance(dMP,d); if(distframePtr->mvKeysUn[p].angle-keyFrame1->framePtr->mvKeysUn[bestIdx2].angle; if(rot<0.0) rot+=360.0f; int bin = round(rot*factor); if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin &matches12,vector &matches21) { const vector &vKeysUn1 = pKF1->framePtr->mvKeysUn; const DBoW2::FeatureVector &vFeatVec1 = pKF1->framePtr->mFeatVec; const cv::Mat &Descriptors1 = pKF1->framePtr->mDescriptors; const vector &vKeysUn2 = pKF2->framePtr->mvKeysUn; const DBoW2::FeatureVector &vFeatVec2 = pKF2->framePtr->mFeatVec; const cv::Mat &Descriptors2 = pKF2->framePtr->mDescriptors; vector rotHist[HISTO_LENGTH]; for(int i=0;ifirst == f2it->first) { for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; if (matches12[idx1]>=0) { nmatches++; assert(0); continue; } if (!pKF1->mvLocalMapPoints[idx1].isEstimated) { continue; } const cv::Mat &d1 = Descriptors1.row(idx1); int bestDist1=256; int bestIdx2 =-1 ; int bestDist2=256; for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; if (matches21[idx2]>=0) { continue; } const cv::Mat &d2 = Descriptors2.row(idx2); int dist = DescriptorDistance(d1,d2); if(dist(bestDist1)(bestDist2)){ matches12[idx1]=bestIdx2; matches21[bestIdx2]=idx1; if(mbCheckOrientation){ float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle; if(rot<0.0) rot+=360.0f; int bin = round(rot*factor); if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && binfirst < f2it->first){ f1it = vFeatVec1.lower_bound(f2it->first); }else{ f2it = vFeatVec2.lower_bound(f1it->first); } } if(mbCheckOrientation) { int ind1=-1; int ind2=-1; int ind3=-1; ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); for(int i=0; i &matches12,vector &matches21){ const vector &vKeysUn1 = pKF1->framePtr->mvKeysUn; const DBoW2::FeatureVector &vFeatVec1 = pKF1->framePtr->mFeatVec; const cv::Mat &Descriptors1 = pKF1->framePtr->mDescriptors; const vector &vKeysUn2 = pKF2->framePtr->mvKeysUn; const DBoW2::FeatureVector &vFeatVec2 = pKF2->framePtr->mFeatVec; const cv::Mat &Descriptors2 = pKF2->framePtr->mDescriptors; vector rotHist[HISTO_LENGTH]; for(int i=0;ifirst == f2it->first) { for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; if (matches12[idx1]>=0) { nmatches++; continue; } if (!pKF1->mvLocalMapPoints[idx1].isEstimated) { continue; } const cv::Mat &d1 = Descriptors1.row(idx1); int bestDist1=256; int bestIdx2 =-1 ; int bestDist2=256; for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; if (matches21[idx2]>=0) { continue; } const cv::Mat &d2 = Descriptors2.row(idx2); int dist = DescriptorDistance(d1,d2); if(dist(bestDist1)(bestDist2)){ matches12[idx1]=bestIdx2; matches21[bestIdx2]=idx1; if(mbCheckOrientation){ float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle; if(rot<0.0) rot+=360.0f; int bin = round(rot*factor); if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && binfirst < f2it->first){ f1it = vFeatVec1.lower_bound(f2it->first); }else{ f2it = vFeatVec2.lower_bound(f1it->first); } } if(mbCheckOrientation) { int ind1=-1; int ind2=-1; int ind3=-1; ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); for(int i=0; i* histo, const int L, int &ind1, int &ind2, int &ind3) { int max1=0; int max2=0; int max3=0; for(int i=0; imax1) { max3=max2; max2=max1; max1=s; ind3=ind2; ind2=ind1; ind1=i; } else if(s>max2) { max3=max2; max2=s; ind3=ind2; ind2=i; } else if(s>max3) { max3=s; ind3=i; } } if(max2<0.1f*(float)max1) { ind2=-1; ind3=-1; } else if(max3<0.1f*(float)max1) { ind3=-1; } } // Bit set count operation from // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b) { const int *pa = a.ptr(); const int *pb = b.ptr(); int dist=0; for(int i=0; i<8; i++, pa++, pb++) { unsigned int v = *pa ^ *pb; v = v - ((v >> 1) & 0x55555555); v = (v & 0x33333333) + ((v >> 2) & 0x33333333); dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; } return dist; } } //namespace ORB_SLAM ================================================ FILE: GSLAM/ORBmatcher.h ================================================ /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */ #ifndef ORBMATCHER_H #define ORBMATCHER_H #include #include "opencv2/core/core.hpp" #include "opencv2/features2d/features2d.hpp" #include"KeyFrame.h" #include"Frame.h" namespace GSLAM { class ORBmatcher{ public: ORBmatcher(float nnratio=0.6, bool checkOri=true); // Computes the Hamming distance between two ORB descriptors static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); int SearchByBoW(KeyFrame *keyFrame1,KeyFrame *keyFrame2,std::vector &matches12,std::vector &matches21); int SearchByBoWLoop(KeyFrame *keyFrame1,KeyFrame *keyFrame2,std::vector &matches12,std::vector &matches21); int SearchByTracking(KeyFrame *keyFrame1,KeyFrame *keyFrame2, std::vector &matches12,std::vector &matches21,int ORBdist); int SearchByProjection(KeyFrame *keyFrame1,KeyFrame *keyFrame2,Transform transform, std::vector &matches12,std::vector &matches21, const float th,const int ORBdist); int SearchByProjectionLoop(KeyFrame *keyFrame1,KeyFrame *keyFrame2,Transform transform, std::vector &matches12,std::vector &matches21, const float th,const int ORBdist); int SearchByRotation(Frame* frame1,Frame* frame2,const cv::Mat& rotation, std::vector& pts1,std::vector& pts2,int ORBdist); //backward projection; int RefineMatchByProjection(KeyFrame *keyFrame1,KeyFrame *keyFrame2,Transform transform, std::vector &matches12,std::vector &matches21, const float th,const int ORBdist); Eigen::Matrix3d K; double projErrorThreshold; public: static const int TH_LOW; static const int TH_HIGH; static const int HISTO_LENGTH; protected: bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF); float RadiusByViewingCos(const float &viewCos); void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3); float mfNNratio; bool mbCheckOrientation; }; }// namespace ORB_SLAM #endif // ORBMATCHER_H ================================================ FILE: GSLAM/RelativeMotion.hpp ================================================ // // RelativeMotion.hpp // GSLAM // // Created by Chaos on 2/17/17. // Copyright © 2017 ctang. All rights reserved. // #ifndef RelativeMotion_h #define RelativeMotion_h #include "ceres/rotation.h" #include "ceres/ceres.h" struct RotationTranslation{ RotationTranslation(const double _x1,const double _y1,const double _z1, const double _x2,const double _y2,const double _z2): x1(_x1),y1(_y1),z1(_z1),x2(_x2),y2(_y2),z2(_z2){ } template bool operator()(const T* const camera, T* residuals) const { T rotated1[3]; T p1[3]={(T)x1,(T)y1,(T)z1}; T p2[3]={(T)x2,(T)y2,(T)z2}; ceres::AngleAxisRotatePoint(camera,p1,rotated1); T norm=camera[3]*camera[3]+camera[4]*camera[4]+camera[5]*camera[5]; if(sqrt(norm)>=(T)std::numeric_limits::min()){//translation is not zero T axis[3]; ceres::CrossProduct(rotated1,&camera[3],axis); T D=-(axis[0]*rotated1[0]+axis[1]*rotated1[1]+axis[2]*rotated1[2]); norm=axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]; T t=(axis[0]*p2[0]+axis[1]*p2[1]+axis[2]*p2[2]+D)/norm; rotated1[0]=p2[0]+axis[0]*t; rotated1[1]=p2[1]+axis[1]*t; rotated1[2]=p2[2]+axis[2]*t; } norm=sqrt(rotated1[0]*rotated1[0]+rotated1[1]*rotated1[1]+rotated1[2]*rotated1[2]); residuals[0]=rotated1[0]/norm-p2[0]; residuals[1]=rotated1[1]/norm-p2[1]; residuals[2]=rotated1[2]/norm-p2[2]; return true; } static ceres::CostFunction* Create(const double x1, const double y1, const double z1, const double x2, const double y2, const double z2) { return (new ceres::AutoDiffCostFunction(new RotationTranslation(x1,y1,z1,x2,y2,z2))); }; double x1; double y1; double z1; double x2; double y2; double z2; }; struct Rotation{ Rotation(const double _x1,const double _y1,const double _z1, const double _x2,const double _y2,const double _z2): x1(_x1),y1(_y1),z1(_z1),x2(_x2),y2(_y2),z2(_z2){ } template bool operator()(const T* const camera, T* residuals) const { T rotated1[3]; T p1[3]={(T)x1,(T)y1,(T)z1}; T p2[3]={(T)x2,(T)y2,(T)z2}; ceres::AngleAxisRotatePoint(camera,p1,rotated1); residuals[0]=rotated1[0]-p2[0]; residuals[1]=rotated1[1]-p2[1]; residuals[2]=rotated1[2]-p2[2]; return true; } static ceres::CostFunction* Create(const double x1, const double y1, const double z1, const double x2, const double y2, const double z2) { return (new ceres::AutoDiffCostFunction(new Rotation(x1,y1,z1,x2,y2,z2))); }; double x1; double y1; double z1; double x2; double y2; double z2; }; Eigen::Vector3d estimateRelativeTranslation(const std::vector &pts1, const std::vector &pts2){ int num_point=pts1.size(); std::vector norms(num_point); Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point); Eigen::Vector3d preResult,curResult; for(int i=0;i svd(NtN,Eigen::ComputeFullV); Eigen::Matrix3d V=svd.matrixV(); curResult=V.col(2); return curResult; } void preSolve(const std::vector &pts1, const std::vector &pts2, const Eigen::Matrix3d& rotation, Eigen::Vector3d& translation){ std::vector _pts1(pts1.size()),_pts2(pts2.size()); for(int p=0;p _pts1(pts1.size()),_pts2(pts2.size()); for(int p=0;p &pts1, const std::vector &pts2, Eigen::Matrix3d& rotation, Eigen::Vector3d& translation){ assert(pts1.size()==pts2.size()); preSolve(pts1,pts2,rotation,translation); double motion[6]={0}; ceres::RotationMatrixToAngleAxis(rotation.data(),&motion[0]); motion[3]=translation(0); motion[4]=translation(1); motion[5]=translation(2); ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(lossThreshold); for(int i=0;i &pts1, const std::vector &pts2, std::vector& ratios, Eigen::Matrix3d& rotation, Eigen::Vector3d& translation){ assert(pts1.size()==pts2.size()); preSolve(pts1,pts2,rotation,translation); double motion[6]={0}; ceres::RotationMatrixToAngleAxis(rotation.data(),&motion[0]); motion[3]=translation(0); motion[4]=translation(1); motion[5]=translation(2); ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(lossThreshold); for(int i=0;i bool operator()(const T* const camera, const T* const radius, T* residuals) const { T p[3]; T point[3]={(T)x1,(T)y1,(T)z1}; T norm=sqrt(camera[3]*camera[3]+camera[4]*camera[4]+camera[5]*camera[5]); ceres::AngleAxisRotatePoint(camera,point,p); p[0]+=(*radius)*(camera[3]-p[0]); p[1]+=(*radius)*(camera[4]-p[1]); p[2]+=(*radius)*(camera[5]-p[2]); norm=sqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]); residuals[0]=p[0]/norm-(T)x2; residuals[1]=p[1]/norm-(T)y2; residuals[2]=p[2]/norm-(T)z2; return true; } static ceres::CostFunction* Create(const double x1, const double y1, const double z1, const double x2, const double y2, const double z2) { return (new ceres::AutoDiffCostFunction(new Transform(x1,y1,z1,x2,y2,z2))); }; double x1; double y1; double z1; double x2; double y2; double z2; }; void estimateRotationTranslation2(const double lossThreshold, const std::vector &pts1, const std::vector &pts2, Eigen::Matrix3d& rotation, Eigen::Vector3d& translation){ double motion[6]={0}; ceres::RotationMatrixToAngleAxis(rotation.data(),&motion[0]); motion[3]=translation(0); motion[4]=translation(1); motion[5]=translation(2); ceres::Problem problem; ceres::LossFunction* loss_function = NULL; std::vector ratios(pts1.size(),0.0); for(int i=0;i #include namespace GSLAM{ System::System(const string &strVocFile, const string &strSettingsFile){ // Output welcome message cout << endl << "GSLAM Copyright (C) 2014-2016 Chengzhou Tang, Simon Fraser University." << endl << "This program comes with ABSOLUTELY NO WARRANTY;" << endl << "This is free software, and you are welcome to redistribute it" << endl << "under certain conditions. See LICENSE.txt." << endl << endl; //Check settings file cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); if(!fsSettings.isOpened()) { cerr << "Failed to open settings file at: " << strSettingsFile << endl; exit(-1); } //Load ORB Vocabulary cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; mpVocabulary = new ORBVocabulary(); bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); if(!bVocLoad) { cerr << "Wrong path to vocabulary. " << endl; cerr << "Falied to open at: " << strVocFile << endl; exit(-1); } cout << "Vocabulary loaded!" << endl << endl; mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); cout << "Keyframe Database created!" << endl << endl; //loading parameter cameraSettings.fx=fsSettings["Camera.fx"]; cameraSettings.fy=fsSettings["Camera.fy"]; cameraSettings.ox=fsSettings["Camera.ox"]; cameraSettings.oy=fsSettings["Camera.oy"]; mK=cv::Mat::zeros(3,3,CV_64FC1); mK.at(0,0) = cameraSettings.fx; mK.at(1,1) = cameraSettings.fy; mK.at(0,2) = cameraSettings.ox; mK.at(1,2) = cameraSettings.oy; cameraSettings.k1=fsSettings["Camera.k1"]; cameraSettings.k2=fsSettings["Camera.k2"]; cameraSettings.p1=fsSettings["Camera.p1"]; cameraSettings.p2=fsSettings["Camera.p2"]; cameraSettings.k3=fsSettings["Camera.k3"]; mDistCoef=cv::Mat(4,1,CV_32F); mDistCoef.at(0) = fsSettings["Camera.k1"]; mDistCoef.at(1) = fsSettings["Camera.k2"]; mDistCoef.at(2) = fsSettings["Camera.p1"]; mDistCoef.at(3) = fsSettings["Camera.p2"]; const float k3 = fsSettings["Camera.k3"]; if(k3!=0){ mDistCoef.resize(5); mDistCoef.at(4) = k3; } int nFeatures = fsSettings["ORBextractor.nFeatures"]; float fScaleFactor= fsSettings["ORBextractor.scaleFactor"]; int nLevels = fsSettings["ORBextractor.nLevels"]; int fIniThFAST = fsSettings["ORBextractor.iniThFAST"]; int fMinThFAST = fsSettings["ORBextractor.minThFAST"]; mpORBExtractor= new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); mpORBExtractorFrame= new ORBextractor(1000,1.2,2,fIniThFAST,fMinThFAST); //orbMatcher=new ORBmatcher(0.8,false); trackingContext=KLTCreateTrackingContext(); trackingContext->window_width=fsSettings["KLT.window_width"]; trackingContext->window_height=fsSettings["KLT.window_height"]; trackingContext->nPyramidLevels=fsSettings["KLT.nPyramidLevels"]; trackingContext->borderx=fsSettings["KLT.borderx"]; trackingContext->bordery=fsSettings["KLT.bordery"]; trackingContext->min_eigenvalue=fsSettings["KLT.min_eigenvalue"]; trackingContext->mindist=fsSettings["KLT.mindist"]; mScaleFactor=std::pow(2,trackingContext->nPyramidLevels-1); mK=mK/mScaleFactor; mK.at(2,2)=1.0; mKInv=mK.inv(); featureList=KLTCreateFeatureList(trackingContext,2000); frameId=0; K=Eigen::Matrix3d::Zero(); K(0,0)=cameraSettings.fx; K(1,1)=cameraSettings.fy; K(2,2)=1.0; K(0,2)=cameraSettings.ox; K(1,2)=cameraSettings.oy; invK=K.inverse(); cvInvK=cv::Mat(3,3,CV_64FC1); for (int r1=0;r1<3;r1++) { for (int r2=0;r2<3;r2++) { cvInvK.at(r1,r2)=invK(r1,r2); } } imu.ts=fsSettings["Camera.ts"]; cameraSettings.td=fsSettings["Camera.td"]; slamSettings.medViewAngle=fsSettings["SLAM.medViewAngle"]; slamSettings.medViewAngle=std::cos((slamSettings.medViewAngle/180.0)*CV_PI); slamSettings.minViewAngle=fsSettings["SLAM.minViewAngle"]; slamSettings.minViewAngle=std::cos((slamSettings.minViewAngle/180.0)*CV_PI); slamSettings.requireNewKeyFrameCount=fsSettings["SLAM.requireNewKeyFrameCount"]; keyFrameConnector.matcherByTracking=new ORBmatcher(0.8,false); keyFrameConnector.matcherByProjection=new ORBmatcher(0.8,false); keyFrameConnector.matcherByBoW=new ORBmatcher(0.75,true); keyFrameConnector.matcherByTracking->K=K/mScaleFactor; keyFrameConnector.matcherByTracking->K(2,2)=1.0; keyFrameConnector.matcherByProjection->K=keyFrameConnector.matcherByTracking->K; lightFramePre=NULL; lightFrameCur=NULL; frameStart=fsSettings["SLAM.startFrame"]; frameEnd=fsSettings["SLAM.endFrame"]; } void processKeyFrame(System& system,KeyFrame& keyFrame){ LocalFactorization localFactorization; localFactorization.process(&keyFrame); //localFactorization.iterativeRefine(keyFrame); #ifdef DEBUG //sprintf(name,"/Users/chaos/Desktop/debug/init_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); #endif LocalBundleAdjustment localBundleAdjustment; localBundleAdjustment.projErrorThres=0.005; localBundleAdjustment.viewAngleThres=system.slamSettings.minViewAngle; //printf("ba\n"); localBundleAdjustment.bundleAdjust(&keyFrame); //sprintf(name,"/Users/chaos/Desktop/debug/ba0_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); localBundleAdjustment.triangulate2(&keyFrame); //sprintf(name,"/Users/chaos/Desktop/debug/tra_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); localBundleAdjustment.bundleAdjust(&keyFrame); //localBundleAdjustment.refinePoints(&keyFrame); //keyFrame->visualize(); //localBundleAdjustment.estimateCloseFrames(keyFrame); //getchar(); #ifdef DEBUG char name[200]; //sprintf(name,"/Users/chaos/Desktop/syndata/data_%d.txt",keyFrame.frameId); //keyFrame.saveData2(name); sprintf(name,"/Users/chaos/Desktop/debug/data_%d.ply",keyFrame.frameId); keyFrame.savePly(name); #endif if(keyFrame.prevKeyFramePtr!=NULL){ if (keyFrame.prevKeyFramePtr->baThread.joinable()) { keyFrame.prevKeyFramePtr->baThread.join(); } system.keyFrameConnector.mpORBVocabulary=system.mpVocabulary; system.keyFrameConnector.keyFrameDatabase=system.mpKeyFrameDatabase; system.keyFrameConnector.connectionThreshold=80; system.keyFrameConnector.connectKeyFrame(keyFrame.prevKeyFramePtr,&keyFrame); } system.mpKeyFrameDatabase->add(&keyFrame); system.globalReconstruction.addNewKeyFrame(&keyFrame); } class NormalizeFrameInvoker{ private: KeyFrame* keyFrame; ImageGrids* grids; KLT_FeatureList* featureList; std::vector* rotations; cv::Mat* cvInvK; public: NormalizeFrameInvoker(KeyFrame* _keyFrame, ImageGrids* _grids, KLT_FeatureList* _featureList, std::vector* _rotations, cv::Mat* _cvInvK){ keyFrame=_keyFrame; grids=_grids; featureList=_featureList; rotations=_rotations; cvInvK=_cvInvK; } void operator ()(const tbb::blocked_range& range) const; }; void NormalizeFrameInvoker::operator()(const tbb::blocked_range& range) const{ for (int i=range.begin();ifeature[i]->val==KLT_TRACKED) { cv::Mat dst; grids->rotateAndNormalizePoint(cv::Point2f((*featureList)->feature[i]->x, (*featureList)->feature[i]->y), dst,*rotations,*cvInvK); (*featureList)->feature[i]->norm(0)=dst.at(0); (*featureList)->feature[i]->norm(1)=dst.at(1); (*featureList)->feature[i]->norm(2)=dst.at(2); (*featureList)->feature[i]->vec=(*featureList)->feature[i]->norm /(*featureList)->feature[i]->norm(2); keyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f((*featureList)->feature[i]->x, (*featureList)->feature[i]->y)); } } } class NormalizeKeyFrameInvoker{ private: KeyFrame* keyFrame; ImageGrids* grids; KLT_FeatureList* featureList; std::vector* rotations; cv::Mat* cvInvK; cv::Mat* colorImage; public: NormalizeKeyFrameInvoker(KeyFrame* _keyFrame, ImageGrids* _grids, KLT_FeatureList* _featureList, std::vector* _rotations, cv::Mat* _cvInvK, cv::Mat* _colorImage){ keyFrame=_keyFrame; grids=_grids; featureList=_featureList; rotations=_rotations; cvInvK=_cvInvK; colorImage=_colorImage; } void operator ()(const tbb::blocked_range& range) const; }; void NormalizeKeyFrameInvoker::operator()(const tbb::blocked_range& range) const{ for(int i=range.begin();irotateAndNormalizePoint(cv::Point2f((*featureList)->feature[i]->x,(*featureList)->feature[i]->y), dst,*rotations,*cvInvK); keyFrame->mvLocalMapPoints[i].norm(0)=dst.at(0); keyFrame->mvLocalMapPoints[i].norm(1)=dst.at(1); keyFrame->mvLocalMapPoints[i].norm(2)=dst.at(2); keyFrame->mvLocalMapPoints[i].vec=keyFrame->mvLocalMapPoints[i].norm /keyFrame->mvLocalMapPoints[i].norm(2); cv::Vec3b color=colorImage->at(round((*featureList)->feature[i]->y/2), round((*featureList)->feature[i]->x/2)); keyFrame->mvLocalMapPoints[i].color[0]=color.val[0]; keyFrame->mvLocalMapPoints[i].color[1]=color.val[1]; keyFrame->mvLocalMapPoints[i].color[2]=color.val[2]; keyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f((*featureList)->feature[i]->x, (*featureList)->feature[i]->y)); } } Transform System::Track(cv::Mat &im, const double ×tamp,const int outIndex){ Transform pose; #ifdef DEBUG static vector colors; static vector keyPoints; trackingContext->min_eigenvalue=100; trackingContext->mindist=10; trackingContext->writeInternalImages = FALSE; trackingContext->affineConsistencyCheck = FALSE; trackingContext->lighting_insensitive=FALSE; trackingContext->window_width=21; trackingContext->window_height=21; trackingContext->nPyramidLevels=2; trackingContext->borderx=50; trackingContext->bordery=50; #endif if (Frame::mbInitialComputations) { //google::InitGoogleLogging("test"); //for get rotation need to change! int frameWidth=im.cols,frameHeight=im.rows; frameSize=cv::Size(frameWidth,frameHeight); cv::Size gridSize(20,20),sizeByGrid; sizeByGrid.width=frameWidth/gridSize.width; sizeByGrid.height=frameHeight/gridSize.height; sizeByVertex=sizeByGrid+cv::Size(1,1); grids.initialize(gridSize,sizeByGrid,sizeByVertex); rotations.resize(sizeByVertex.height); frameIndex=0; globalRotations.clear(); frameStamp=timestamp+cameraSettings.td; imu.getIntraFrameRotation(rotations,grids.originGridVertices,frameSize,sizeByVertex,frameIndex,frameStamp); rotations[rotations.size()-1].copyTo(rotation); globalRotations.push_back(cv::Mat::eye(3,3,CV_64FC1)); //create klt frame pyramidBuffers.initialize(); computePyramid(trackingContext,im,*pyramidBuffers.ptrs[1]); pyramidBuffers.preloadThread=std::thread(computePyramid, std::ref(trackingContext), std::ref(*preloadImage), std::ref(*pyramidBuffers.ptrs[2])); //char name[200]; //printf(name,"/Users/ctang/edge.png"); /*cv::Mat normx; cv::Mat normy; cv::Mat normxy; cv::multiply(pyramid1[1],pyramid1[1],normx); cv::multiply(pyramid1[2],pyramid1[2],normy); cv::sqrt(normx+normy,normxy); cv::imwrite("/Users/ctang/edge.png",normxy);getchar();*/ /*cv::Mat gx,gy,gxx,gyy,gxy,sumGxx,sumGyy,sumGxy; cv::multiply(pyramid1[1],pyramid1[1],gxx); cv::multiply(pyramid1[2],pyramid1[2],gyy); cv::multiply(pyramid1[1],pyramid1[2],gxy); cv::integral(gxx,sumGxx,CV_32F); cv::integral(gyy,sumGyy,CV_32F); cv::integral(gxy,sumGxy,CV_32F); KLTSelectGoodFeatures(trackingContext,sumGxx,sumGyy,sumGxy,featureList);*/ /*pyramid1Ptr=&pyramid1; pyramid2Ptr=&pyramid2; pyramid3Ptr=&pyramid3; pyramidTmpPtr=NULL;*/ //create orb frame and keyframe cv::Mat ucharImage; //pyramid1[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1); (*pyramidBuffers.ptrs[1])[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1); Frame *framePtr=new Frame(ucharImage,timestamp,mpORBExtractor,mpVocabulary,mK,mDistCoef); KeyFrame *keyFramePtr=new KeyFrame(framePtr,mpKeyFrameDatabase,invK,mScaleFactor); keyFramePtr->frameId=frameId; keyFramePtr->outId=outIndex; for(int i=0;imvLocalMapPoints.size();i++){ featureList->feature[i]->x=keyFramePtr->framePtr->mvKeys[i].pt.x*mScaleFactor; featureList->feature[i]->y=keyFramePtr->framePtr->mvKeys[i].pt.y*mScaleFactor; featureList->feature[i]->val=KLT_TRACKED; } featureList->nFeatures=keyFramePtr->mvLocalMapPoints.size(); frontKeyFrame=keyFramePtr; preKeyFrame=NULL; /*for(int i=0;imvLocalMapPoints.size();i++){ cv::Mat dst; grids.rotateAndNormalizePoint(cv::Point2f(featureList->feature[i]->x,featureList->feature[i]->y), dst,rotations,cvInvK); frontKeyFrame->mvLocalMapPoints[i].norm(0)=dst.at(0); frontKeyFrame->mvLocalMapPoints[i].norm(1)=dst.at(1); frontKeyFrame->mvLocalMapPoints[i].norm(2)=dst.at(2); frontKeyFrame->mvLocalMapPoints[i].vec=frontKeyFrame->mvLocalMapPoints[i].norm /frontKeyFrame->mvLocalMapPoints[i].norm(2); cv::Vec3b color=colorImage->at(round(featureList->feature[i]->y/2),round(featureList->feature[i]->x/2)); frontKeyFrame->mvLocalMapPoints[i].color[0]=color.val[0]; frontKeyFrame->mvLocalMapPoints[i].color[1]=color.val[1]; frontKeyFrame->mvLocalMapPoints[i].color[2]=color.val[2]; frontKeyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f(featureList->feature[i]->x, featureList->feature[i]->y)); }*/ tbb::parallel_for(tbb::blocked_range(0,frontKeyFrame->mvLocalMapPoints.size()),NormalizeKeyFrameInvoker(frontKeyFrame,&grids,&featureList,&rotations,&cvInvK,colorImage)); //lightFramePre=new Frame(ucharImage,timestamp,mpORBExtractorFrame,mpVocabulary,mK,mDistCoef); }else{ /* cv::Mat ucharImage; (*pyramid2Ptr)[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1); lightFrameCur=new Frame(ucharImage,timestamp,mpORBExtractorFrame,mpVocabulary,mK,mDistCoef); */ frameStamp=timestamp+cameraSettings.td; imu.getInterFrameRotation(rotation,frameIndex,preFrameStamp,frameStamp); globalRotations.push_back(rotation*globalRotations[frameId-1]); /* std::vector pts1,pts2; cv::Mat transform=mK*rotation*mKInv; orbMatcher->SearchByRotation(lightFramePre,lightFrameCur,transform,pts1,pts2,48); std::vector intensity1(pts1.size()),intensity2(pts2.size()); for (int p=0;pnPyramidLevels-1)].at((int)pts1[p].y, (int)pts1[p].x); intensity2[p]=(*pyramid2Ptr)[3*(trackingContext->nPyramidLevels-1)].at((int)pts2[p].y, (int)pts2[p].x); } compensateLightingAndMotion(pts1,pts2,intensity1,intensity2); */ /*for(int i=0;i<9;i++){ std::cout<(i)<nPyramidLevels-1)],debugImage,CV_GRAY2BGR); for (int p=0;pnFeatures;i++) { if (featureList->feature[i]->val==KLT_TRACKED) { cv::Mat dst; grids.rotateAndNormalizePoint(cv::Point2f(featureList->feature[i]->x, featureList->feature[i]->y), dst,rotations,cvInvK); featureList->feature[i]->norm(0)=dst.at(0); featureList->feature[i]->norm(1)=dst.at(1); featureList->feature[i]->norm(2)=dst.at(2); featureList->feature[i]->vec=featureList->feature[i]->norm /featureList->feature[i]->norm(2); frontKeyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f(featureList->feature[i]->x, featureList->feature[i]->y)); } }*/ tbb::parallel_for(tbb::blocked_range(0,featureList->nFeatures), NormalizeFrameInvoker(frontKeyFrame,&grids,&featureList,&rotations,&cvInvK)); KeyFrame* keyFrame=frontKeyFrame; int pVectorCount=0,fullTrackCount=0,inlierCount=0; std::vector isValid(keyFrame->mvLocalMapPoints.size()); for (int i=0;imvLocalMapPoints.size();i++) { isValid[i]=keyFrame->mvLocalMapPoints[i].isFullTrack; } RelativeOutlierRejection outlierRejection; outlierRejection.K=K; for (int i1=0;i1<3;i1++) { for (int i2=0;i2<3;i2++) { outlierRejection.rotation(i1,i2)=globalRotations[globalRotations.size()-1].at(i1,i2); } } outlierRejection.rotation=outlierRejection.rotation*keyFrame->pose.rotation.transpose(); outlierRejection.theshold=10.0; outlierRejection.prob=0.98; outlierRejection.medViewAngle=slamSettings.medViewAngle; outlierRejection.minViewAngle=slamSettings.minViewAngle; outlierRejection.isValid=&isValid; inlierCount=outlierRejection.relativeRansac(keyFrame,featureList); RelativePoseEstimation relativeEstimation; std::vector pVectors; /*static Eigen::Matrix3d prevRotation; static Eigen::Vector3d prevTranslation; if (keyFrame->mvLocalFrames.size()==0) { outlierRejection.rotation=Eigen::Matrix3d::Identity(); prevTranslation=Eigen::Vector3d::Zero(); }else{ outlierRejection.rotation=prevRotation; }*/ if(inlierCount>0){ relativeEstimation.isValid=&isValid; relativeEstimation.rotation=outlierRejection.rotation; //relativeEstimation.translation=prevTranslation; relativeEstimation.rotationIsKnown=true; relativeEstimation.medViewAngle=slamSettings.medViewAngle; relativeEstimation.minViewAngle=slamSettings.minViewAngle; pVectorCount=relativeEstimation.estimateRelativePose(keyFrame,featureList,pVectors); } //prevRotation=relativeEstimation.rotation; //prevTranslation=relativeEstimation.translation; bool requestNewKeyFrame=false; bool isLastKeyFrame=false; if (pVectorCount>0) { fullTrackCount=keyFrame->appendRelativeEstimation(frameId,NULL, relativeEstimation.rotation, relativeEstimation.translation, featureList,pVectors); requestNewKeyFrame=fullTrackCountmvLocalFrames.size()>15; //requestNewKeyFrame=(requestNewKeyFrame||keyFrame->mvLocalFrames.size()>40); isLastKeyFrame=true; } if(requestNewKeyFrame){ printf("new %d old %d\n",frameId,frontKeyFrame->frameId); char name[200]; /*LocalFactorization localFactorization; localFactorization.process(keyFrame); //localFactorization.iterativeRefine(keyFrame); #ifdef DEBUG //sprintf(name,"/Users/chaos/Desktop/debug/init_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); #endif LocalBundleAdjustment localBundleAdjustment; localBundleAdjustment.projErrorThres=0.005; localBundleAdjustment.viewAngleThres=slamSettings.minViewAngle; //printf("ba\n"); localBundleAdjustment.bundleAdjust(keyFrame); //sprintf(name,"/Users/chaos/Desktop/debug/ba0_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); localBundleAdjustment.triangulate2(keyFrame); //sprintf(name,"/Users/chaos/Desktop/debug/tra_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); //localBundleAdjustment.bundleAdjust(keyFrame); localBundleAdjustment.refinePoints(keyFrame); //keyFrame->visualize(); //localBundleAdjustment.estimateCloseFrames(keyFrame); //getchar(); #ifdef DEBUG //sprintf(name,"/Users/chaos/Desktop/debug/ba1_%d_%d.ply",frontKeyFrame->frameId,frameId); //keyFrame->savePly(name); //getchar(); #endif if(preKeyFrame!=NULL){ keyFrameConnector.mpORBVocabulary=mpVocabulary; keyFrameConnector.keyFrameDatabase=mpKeyFrameDatabase; keyFrameConnector.connectionThreshold=80; keyFrameConnector.connectKeyFrame(preKeyFrame,keyFrame); }*/ keyFrame->prevKeyFramePtr=preKeyFrame; keyFrame->baThread=std::thread(processKeyFrame,std::ref(*this),std::ref(*keyFrame)); //processKeyFrame(*this,*keyFrame); //mpKeyFrameDatabase->add(keyFrame); //globalReconstruction.addNewKeyFrame(keyFrame); cv::Mat ucharImage; (*pyramid2Ptr)[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1); Frame *framePtr=new Frame(ucharImage,timestamp,mpORBExtractor,mpVocabulary,mK,mDistCoef); KeyFrame *keyFramePtr=new KeyFrame(framePtr,mpKeyFrameDatabase,invK,mScaleFactor); keyFramePtr->frameId=frameId; keyFramePtr->outId=outIndex; //set rotation and derolling shutter for (int i1=0;i1<3;i1++) { for (int i2=0;i2<3;i2++) { keyFramePtr->pose.rotation(i1,i2)=globalRotations[globalRotations.size()-1].at(i1,i2); } } for(int i=0;imvLocalMapPoints.size();i++){ featureList->feature[i]->x=keyFramePtr->framePtr->mvKeys[i].pt.x*mScaleFactor; featureList->feature[i]->y=keyFramePtr->framePtr->mvKeys[i].pt.y*mScaleFactor; featureList->feature[i]->val=KLT_TRACKED; } featureList->nFeatures=keyFramePtr->mvLocalMapPoints.size(); /*for(int i=0;imvLocalMapPoints.size();i++){ cv::Mat dst; grids.rotateAndNormalizePoint(cv::Point2f(featureList->feature[i]->x, featureList->feature[i]->y), dst,rotations,cvInvK); keyFramePtr->mvLocalMapPoints[i].norm(0)=dst.at(0); keyFramePtr->mvLocalMapPoints[i].norm(1)=dst.at(1); keyFramePtr->mvLocalMapPoints[i].norm(2)=dst.at(2); keyFramePtr->mvLocalMapPoints[i].vec=keyFramePtr->mvLocalMapPoints[i].norm /keyFramePtr->mvLocalMapPoints[i].norm(2); cv::Vec3b color=colorImage->at(round(featureList->feature[i]->y/2), round(featureList->feature[i]->x/2)); keyFramePtr->mvLocalMapPoints[i].color[0]=color.val[0]; keyFramePtr->mvLocalMapPoints[i].color[1]=color.val[1]; keyFramePtr->mvLocalMapPoints[i].color[2]=color.val[2]; keyFramePtr->mvLocalMapPoints[i].tracked.push_back(cv::Point2f(featureList->feature[i]->x, featureList->feature[i]->y)); }*/ tbb::parallel_for(tbb::blocked_range(0,keyFramePtr->mvLocalMapPoints.size()),NormalizeKeyFrameInvoker(keyFramePtr,&grids,&featureList,&rotations,&cvInvK,colorImage)); preKeyFrame=frontKeyFrame; frontKeyFrame=keyFramePtr; frontKeyFrame->prevKeyFramePtr=preKeyFrame; } printf("frame %d end\n",frameId); pyramidTmpPtr=pyramid1Ptr; pyramid1Ptr=pyramid2Ptr; pyramid2Ptr=pyramidTmpPtr; //delete lightFramePre; //lightFramePre=lightFrameCur; } preFrameStamp=frameStamp; frameId++; return pose; } void System::finish(){ globalReconstruction.path=path; pyramidBuffers.preloadThread.join(); if(frontKeyFrame->mvLocalFrames.size()>0){ LocalFactorization localFactorization; localFactorization.process(frontKeyFrame); LocalBundleAdjustment localBundleAdjustment; localBundleAdjustment.projErrorThres=0.005; localBundleAdjustment.viewAngleThres=slamSettings.minViewAngle; localBundleAdjustment.bundleAdjust(frontKeyFrame); localBundleAdjustment.triangulate(frontKeyFrame); localBundleAdjustment.bundleAdjust(frontKeyFrame); char name[200]; sprintf(name,"/Users/chaos/Desktop/syndata/data_%d.txt",frontKeyFrame->frameId); frontKeyFrame->saveData2(name); sprintf(name,"/Users/chaos/Desktop/syndata/data_%d.ply",frontKeyFrame->frameId); frontKeyFrame->savePly(name); if(preKeyFrame->baThread.joinable()){ preKeyFrame->baThread.join(); } keyFrameConnector.mpORBVocabulary=mpVocabulary; keyFrameConnector.keyFrameDatabase=mpKeyFrameDatabase; keyFrameConnector.connectionThreshold=80; keyFrameConnector.connectKeyFrame(preKeyFrame,frontKeyFrame); mpKeyFrameDatabase->add(frontKeyFrame); globalReconstruction.addNewKeyFrame(frontKeyFrame); } //globalReconstruction.estimateSIM3(); //globalReconstruction.savePly(); //globalReconstruction.savePly(); globalReconstruction.scaleThreshold=20; globalReconstruction.estimateScale(); std::vector index; globalReconstruction.estimateRotation(index); printf("rotation estiamted\n"); globalReconstruction.estimateTranslation(index); globalReconstruction.globalRefine(); globalReconstruction.savePly(); //globalReconstruction.frameStart=frameStart; globalReconstruction.visualize(); //globalReconstruction.topview(); //globalReconstruction.savePly(); //globalReconstruction.topview() //globalReconstruction.savePly(); //globalReconstruction.visualize(); //globalReconstruction.savePly(); //globalReconstruction.globalPBA(); //globalReconstruction.estimateSIM3(); } } ================================================ FILE: GSLAM/System.h ================================================ // // System.h // GSLAM // // Created by ctang on 8/30/16. // Copyright © 2016 ctang. All rights reserved. // #include "opencv2/core/core.hpp" #include "ORBVocabulary.h" #include "KeyFrameDatabase.h" #include "KeyFrameConnection.h" #include "ORBmatcher.h" #include "KLT.h" //#include "IMU.hpp" #include "ImageGrids.hpp" #include "GlobalReconstruction.h" #include #include using namespace std; namespace GSLAM{ class PyramidBuffer{ public: std::thread preloadThread; std::vector* ptrs[3]; std::vector buffers[3]; void initialize(){ buffers[0]=std::vector(); buffers[1]=std::vector(); buffers[2]=std::vector(); ptrs[0]=&buffers[0]; ptrs[1]=&buffers[1]; ptrs[2]=&buffers[2]; } void next(){ if(preloadThread.joinable()){ preloadThread.join(); } std::vector* tmp=ptrs[0]; ptrs[0]=ptrs[1]; ptrs[1]=ptrs[2]; ptrs[2]=tmp; } }; class System{ public: System(const string &strVocFile, const string &strSettingsFile); Transform Track(cv::Mat &im, const double ×tamp,const int outIndex); void finish(); IMU imu; cv::Mat *colorImage; char* path; int frameStart; int frameEnd; KeyFrame* frontKeyFrame; KeyFrame* preKeyFrame; SLAMSettings slamSettings; KeyFrameConnection keyFrameConnector; ORBVocabulary* mpVocabulary; KeyFrameDatabase* mpKeyFrameDatabase; GlobalReconstruction globalReconstruction; cv::Mat* preloadImage; private: // int frameId; //for orb cv::Mat mK; cv::Mat mKInv; cv::Mat mDistCoef; double mScaleFactor; ORBextractor *mpORBExtractor; ORBmatcher *orbMatcher; //for klt tracking PyramidBuffer pyramidBuffers; std::vector *pyramid1Ptr,*pyramid2Ptr,*pyramid3Ptr,*pyramidTmpPtr,pyramid1,pyramid2,pyramid3; KLT_TrackingContext trackingContext; KLT_FeatureList featureList; //for geometry cv::Mat cvInvK; Eigen::Matrix3d K,invK; std::list activeKeyFrames; //KeyFrame* frontKeyFrame; //KeyFrame* preKeyFrame; std::vector matchesToPre; //parameters CameraSettings cameraSettings; ORBExtractorSettings orbExtractorSettings; ///SLAMSettings slamSettings; //temporal parameter for imu ImageGrids grids; std::vector rotations; cv::Mat rotation; int frameIndex; double frameStamp; double preFrameStamp; cv::Size frameSize; std::vector globalRotations; cv::Size sizeByVertex; //keyframe connection //KeyFrameConnection keyFrameConnector; //globalReconstruction //motion lighting compensate ORBextractor *mpORBExtractorFrame; Frame *lightFramePre,*lightFrameCur; }; } ================================================ FILE: GSLAM/TrackUtil.cpp ================================================ // // TrackUtil.cpp // GSLAM // // Created by ctang on 9/10/16. // Copyright © 2016 ctang. All rights reserved. // #include "TrackUtil.h" ================================================ FILE: GSLAM/TrackUtil.h ================================================ // // TrackUtil.h // GSLAM // // Created by ctang on 9/10/16. // Copyright © 2016 ctang. All rights reserved. // #include "KLT.h" ================================================ FILE: GSLAM/Transform.cpp ================================================ // // Transform.cpp // GSLAM // // Created by ctang on 9/5/16. // Copyright © 2016 ctang. All rights reserved. // #include "Transform.h" #include "Geometry.h" #include namespace GSLAM{ Transform::Transform(){ scale=1.0; rotation.setIdentity(); translation.setZero(); } void Transform::fromCameraToWorld(){ } void Transform::fromWorldToCamera(){ } void Transform::setEssentialMatrix(){ setCrossMatrix(E,translation); E=rotation*E; E/=E(2,2); } Transform Transform::leftMultiply(const Transform& transform)const{ Transform result; result.rotation=transform.rotation*rotation; result.translation=rotation.transpose()*transform.translation+translation; return result; } Transform Transform::inverse()const{ Transform result; result.rotation=rotation.transpose(); result.translation=-rotation*translation; return result; } void Transform::display() const{ std::cout< #include #include "opencv2/imgproc/imgproc.hpp" inline void computeGxGy(const cv::Mat& img,cv::Mat& grad,cv::Mat& filter1,cv::Mat& filter2){ cv::sepFilter2D(img,grad,CV_32F,filter1,filter2); } void computeGradients(const cv::Mat &img, cv::Mat &gradx, cv::Mat &grady, float sigma, std::thread* threads){ /* Compute kernels, if necessary */ if (fabs(sigma - sigma_last) > 0.05){ computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); } //std::cout< 0.05){ computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); } cv::Mat kernel(1,gauss_kernel.width,CV_32F,gauss_kernel.data); cv::sepFilter2D(img,dst,-1,kernel,kernel); } /*void sperableFilter2D(cv::Mat& img,cv::Mat& output,cv::Mat& rowFilter,cv::Mat& colFilter){ } void computeGradients2(cv::Mat &img, cv::Mat &gradx, cv::Mat &grady, float sigma){ if (fabs(sigma - sigma_last) > 0.05){ computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); } cv::Mat kernelGauss(1,gauss_kernel.width,CV_32F,gauss_kernel.data); cv::Mat kernelGaussDeriv(1,gaussderiv_kernel.width,CV_32F,gaussderiv_kernel.data); sperableFilter2D(img,gradx,kernelGaussDeriv,kernelGauss); sperableFilter2D(img,grady,kernelGauss,kernelGaussDeriv); } void computeSmoothedImage2(cv::Mat &img, cv::Mat &dst, float sigma){ if (fabs(sigma - sigma_last) > 0.05){ computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel); } cv::Mat kernel(1,gauss_kernel.width,CV_32F,gauss_kernel.data); sperableFilter2D(img,dst,kernel,kernel); }*/ ================================================ FILE: GSLAM/convolve.h ================================================ // // convolve.hpp // STARCK // // Created by Chaos on 3/29/16. // Copyright © 2016 Chaos. All rights reserved. // #include "KLT.h" #include void computeGradients(const cv::Mat &img,cv::Mat &gradx,cv::Mat &grady,float sigma,std::thread* threads=NULL); void computeSmoothedImage(const cv::Mat &img,cv::Mat &dst,float sigma); //void computeGradients2(cv::Mat &img,cv::Mat &gradx,cv::Mat &grady,float sigma); //void computeSmoothedImage2(cv::Mat &img,cv::Mat &dst,float sigma); ================================================ FILE: GSLAM/error.cpp ================================================ /********************************************************************* * error.c * * Error and warning messages, and system commands. *********************************************************************/ /* Standard includes */ #include #include #include /********************************************************************* * KLTError * * Prints an error message and dies. * * INPUTS * exactly like printf */ void KLTError(char *fmt, ...) { va_list args; va_start(args, fmt); fprintf(stderr, "KLT Error: "); vfprintf(stderr, fmt, args); fprintf(stderr, "\n"); va_end(args); exit(1); } /********************************************************************* * KLTWarning * * Prints a warning message. * * INPUTS * exactly like printf */ void KLTWarning(char *fmt, ...) { va_list args; va_start(args, fmt); fprintf(stderr, "KLT Warning: "); vfprintf(stderr, fmt, args); fprintf(stderr, "\n"); fflush(stderr); va_end(args); } ================================================ FILE: GSLAM/error.h ================================================ /********************************************************************* * error.h *********************************************************************/ #include #include void KLTError(char *fmt, ...); void KLTWarning(char *fmt, ...); ================================================ FILE: GSLAM/experiment_helpers.hpp ================================================ /****************************************************************************** * Author: Laurent Kneip * * Contact: kneip.laurent@gmail.com * * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. * * * * Redistribution and use in source and binary forms, with or without * * modification, are permitted provided that the following conditions * * are met: * * * Redistributions of source code must retain the above copyright * * notice, this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright * * notice, this list of conditions and the following disclaimer in the * * documentation and/or other materials provided with the distribution. * * * Neither the name of ANU nor the names of its contributors may be * * used to endorse or promote products derived from this software without * * specific prior written permission. * * * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * * SUCH DAMAGE. * ******************************************************************************/ #ifndef OPENGV_EXPERIMENT_HELPERS_HPP_ #define OPENGV_EXPERIMENT_HELPERS_HPP_ #include #include namespace opengv { void generateCentralCameraSystem( translations_t & camOffsets, rotations_t & camRotations ); void generateRandomCameraSystem( int numberCameras, translations_t & camOffsets, rotations_t & camRotations ); void extractRelativePose( const translation_t & position1, const translation_t & position2, const rotation_t & rotation1, const rotation_t & rotation2, translation_t & relativePosition, rotation_t & relativeRotation, bool normalize = true ); void printExperimentCharacteristics( const translation_t & position, const rotation_t & rotation, double noise, double outlierFraction ); void printBearingVectorArraysMatlab( const bearingVectors_t & bearingVectors1, const bearingVectors_t & bearingVectors2 ); void printEssentialMatrix( const translation_t & position, const rotation_t & rotation); void getPerturbedPose( const translation_t & position, const rotation_t & rotation, translation_t & perturbedPosition, rotation_t & perturbedRotation, double amplitude ); std::vector getNindices( int n ); void generateRandom2D3DCorrespondences( const translation_t & position, const rotation_t & rotation, const translations_t & camOffsets, const rotations_t & camRotations, size_t numberPoints, double noise, double outlierFraction, bearingVectors_t & bearingVectors, points_t & points, std::vector & camCorrespondences, Eigen::MatrixXd & gt ); void generateMulti2D3DCorrespondences( const translation_t & position, const rotation_t & rotation, const translations_t & camOffsets, const rotations_t & camRotations, size_t pointsPerCam, double noise, double outlierFraction, std::vector > & multiPoints, std::vector > & multiBearingVectors, std::vector > & gt ); void generateRandom2D2DCorrespondences( const translation_t & position1, const rotation_t & rotation1, const translation_t & position2, const rotation_t & rotation2, const translations_t & camOffsets, const rotations_t & camRotations, size_t numberPoints, double noise, double outlierFraction, bearingVectors_t & bearingVectors1, bearingVectors_t & bearingVectors2, std::vector & camCorrespondences1, std::vector & camCorrespondences2, Eigen::MatrixXd & gt ); void generateRandom3D3DCorrespondences( const translation_t & position1, const rotation_t & rotation1, const translation_t & position2, const rotation_t & rotation2, size_t numberPoints, double noise, double outlierFraction, bearingVectors_t & points1, bearingVectors_t & points2, Eigen::MatrixXd & gt ); void generateMulti2D2DCorrespondences( const translation_t & position1, const rotation_t & rotation1, const translation_t & position2, const rotation_t & rotation2, const translations_t & camOffsets, const rotations_t & camRotations, size_t pointsPerCam, double noise, double outlierFraction, std::vector > & multiBearingVectors1, std::vector > & multiBearingVectors2, std::vector > & gt ); } #endif /* OPENGV_EXPERIMENT_HELPERS_HPP_ */ ================================================ FILE: GSLAM/las2.cpp ================================================ /* Copyright © 2002, University of Tennessee Research Foundation. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the University of Tennessee nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include #include "svdlib.hpp" #include "svdutil.hpp" #define MAXLL 2 #define LMTNW 100000000 /* max. size of working area allowed */ enum storeVals {STORQ = 1, RETRQ, STORP, RETRP}; static char *error_msg[] = { /* error messages used by function * * check_parameters */ NULL, "", "ENDL MUST BE LESS THAN ENDR", "REQUESTED DIMENSIONS CANNOT EXCEED NUM ITERATIONS", "ONE OF YOUR DIMENSIONS IS LESS THAN OR EQUAL TO ZERO", "NUM ITERATIONS (NUMBER OF LANCZOS STEPS) IS INVALID", "REQUESTED DIMENSIONS (NUMBER OF EIGENPAIRS DESIRED) IS INVALID", "6*N+4*ITERATIONS+1 + ITERATIONS*ITERATIONS CANNOT EXCEED NW", "6*N+4*ITERATIONS+1 CANNOT EXCEED NW", NULL}; double **LanStore, *OPBTemp; double eps, eps1, reps, eps34; long ierr; /* double rnm, anorm, tol; FILE *fp_out1, *fp_out2; */ void purge(long n, long ll, double *r, double *q, double *ra, double *qa, double *wrk, double *eta, double *oldeta, long step, double *rnmp, double tol); void ortbnd(double *alf, double *eta, double *oldeta, double *bet, long step, double rnm); double startv(SMat A, double *wptr[], long step, long n); void store(long, long, long, double *); void imtql2(long, long, double *, double *, double *); void imtqlb(long n, double d[], double e[], double bnd[]); void write_header(long, long, double, double, long, double, long, long, long); long check_parameters(SMat A, long dimensions, long iterations, double endl, double endr, long vectors); int lanso(SMat A, long iterations, long dimensions, double endl, double endr, double *ritz, double *bnd, double *wptr[], long *neigp, long n); long ritvec(long n, SMat A, SVDRec R, double kappa, double *ritz, double *bnd, double *alf, double *bet, double *w2, long steps, long neig); long lanczos_step(SMat A, long first, long last, double *wptr[], double *alf, double *eta, double *oldeta, double *bet, long *ll, long *enough, double *rnmp, double *tolp, long n); void stpone(SMat A, double *wrkptr[], double *rnmp, double *tolp, long n); long error_bound(long *, double, double, double *, double *, long step, double tol); void machar(long *ibeta, long *it, long *irnd, long *machep, long *negep); /*********************************************************************** * * * main() * * Sparse SVD(A) via Eigensystem of A'A symmetric Matrix * * (double precision) * * * ***********************************************************************/ /*********************************************************************** Description ----------- This sample program uses landr to compute singular triplets of A via the equivalent symmetric eigenvalue problem B x = lambda x, where x' = (u',v'), lambda = sigma**2, where sigma is a singular value of A, B = A'A , and A is m (nrow) by n (ncol) (nrow >> ncol), so that {u,sqrt(lambda),v} is a singular triplet of A. (A' = transpose of A) User supplied routines: svd_opa, opb, store, timer svd_opa( x,y) takes an n-vector x and returns A*x in y. svd_opb(ncol,x,y) takes an n-vector x and returns B*x in y. Based on operation flag isw, store(n,isw,j,s) stores/retrieves to/from storage a vector of length n in s. User should edit timer() with an appropriate call to an intrinsic timing routine that returns elapsed user time. External parameters ------------------- Defined and documented in las2.h Local parameters ---------------- (input) endl left end of interval containing unwanted eigenvalues of B endr right end of interval containing unwanted eigenvalues of B kappa relative accuracy of ritz values acceptable as eigenvalues of B vectors is not equal to 1 r work array n dimension of the eigenproblem for matrix B (ncol) dimensions upper limit of desired number of singular triplets of A iterations upper limit of desired number of Lanczos steps nnzero number of nonzeros in A vectors 1 indicates both singular values and singular vectors are wanted and they can be found in output file lav2; 0 indicates only singular values are wanted (output) ritz array of ritz values bnd array of error bounds d array of singular values memory total memory allocated in bytes to solve the B-eigenproblem Functions used -------------- BLAS svd_daxpy, svd_dscal, svd_ddot USER svd_opa, svd_opb, timer MISC write_header, check_parameters LAS2 landr Precision --------- All floating-point calculations are done in double precision; variables are declared as long and double. LAS2 development ---------------- LAS2 is a C translation of the Fortran-77 LAS2 from the SVDPACK library written by Michael W. Berry, University of Tennessee, Dept. of Computer Science, 107 Ayres Hall, Knoxville, TN, 37996-1301 31 Jan 1992: Date written Theresa H. Do University of Tennessee Dept. of Computer Science 107 Ayres Hall Knoxville, TN, 37996-1301 internet: tdo@cs.utk.edu ***********************************************************************/ /*********************************************************************** * * * check_parameters() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function validates input parameters and returns error code (long) Parameters ---------- (input) dimensions upper limit of desired number of eigenpairs of B iterations upper limit of desired number of lanczos steps n dimension of the eigenproblem for matrix B endl left end of interval containing unwanted eigenvalues of B endr right end of interval containing unwanted eigenvalues of B vectors 1 indicates both eigenvalues and eigenvectors are wanted and they can be found in lav2; 0 indicates eigenvalues only nnzero number of nonzero elements in input matrix (matrix A) ***********************************************************************/ long check_parameters(SMat A, long dimensions, long iterations, double endl, double endr, long vectors) { long error_index; error_index = 0; if (endl >/*=*/ endr) error_index = 2; else if (dimensions > iterations) error_index = 3; else if (A->cols <= 0 || A->rows <= 0) error_index = 4; /*else if (n > A->cols || n > A->rows) error_index = 1;*/ else if (iterations <= 0 || iterations > A->cols || iterations > A->rows) error_index = 5; else if (dimensions <= 0 || dimensions > iterations) error_index = 6; if (error_index) svd_error("svdLAS2 parameter error: %s\n", error_msg[error_index]); return(error_index); } /*********************************************************************** * * * write_header() * * Function writes out header of output file containing ritz values * * * ***********************************************************************/ void write_header(long iterations, long dimensions, double endl, double endr, long vectors, double kappa, long nrow, long ncol, long vals) { printf("SOLVING THE [A^TA] EIGENPROBLEM\n"); printf("NO. OF ROWS = %6ld\n", nrow); printf("NO. OF COLUMNS = %6ld\n", ncol); printf("NO. OF NON-ZERO VALUES = %6ld\n", vals); printf("MATRIX DENSITY = %6.2f%%\n", ((float) vals / nrow) * 100 / ncol); /* printf("ORDER OF MATRIX A = %5ld\n", n); */ printf("MAX. NO. OF LANCZOS STEPS = %6ld\n", iterations); printf("MAX. NO. OF EIGENPAIRS = %6ld\n", dimensions); printf("LEFT END OF THE INTERVAL = %9.2E\n", endl); printf("RIGHT END OF THE INTERVAL = %9.2E\n", endr); printf("KAPPA = %9.2E\n", kappa); /* printf("WANT S-VECTORS? [T/F] = %c\n", (vectors) ? 'T' : 'F'); */ printf("\n"); return; } /*********************************************************************** * * * landr() * * Lanczos algorithm with selective orthogonalization * * Using Simon's Recurrence * * (double precision) * * * ***********************************************************************/ /*********************************************************************** Description ----------- landr() is the LAS2 driver routine that, upon entry, (1) checks for the validity of input parameters of the B-eigenproblem (2) determines several machine constants (3) makes a Lanczos run (4) calculates B-eigenvectors (singular vectors of A) if requested by user arguments --------- (input) n dimension of the eigenproblem for A'A iterations upper limit of desired number of Lanczos steps dimensions upper limit of desired number of eigenpairs nnzero number of nonzeros in matrix A endl left end of interval containing unwanted eigenvalues of B endr right end of interval containing unwanted eigenvalues of B vectors 1 indicates both eigenvalues and eigenvectors are wanted and they can be found in output file lav2; 0 indicates only eigenvalues are wanted kappa relative accuracy of ritz values acceptable as eigenvalues of B (singular values of A) r work array (output) j number of Lanczos steps actually taken neig number of ritz values stabilized ritz array to hold the ritz values bnd array to hold the error bounds External parameters ------------------- Defined and documented in las2.h local parameters ------------------- ibeta radix for the floating-point representation it number of base ibeta digits in the floating-point significand irnd floating-point addition rounded or chopped machep machine relative precision or round-off error negeps largest negative integer wptr array of pointers each pointing to a work space Functions used -------------- MISC svd_dmax, machar, check_parameters LAS2 ritvec, lanso ***********************************************************************/ SVDRec svdLAS2A(SMat A, long dimensions) { double end[2] = {-1.0e-30, 1.0e-30}; double kappa = 1e-30; if (!A) { svd_error("svdLAS2A called with NULL array\n"); return NULL; } return svdLAS2(A, dimensions, 0, end, kappa); } SVDRec svdLAS2(SMat A, long dimensions, long iterations, double end[2], double kappa) { char transpose = FALSE; long ibeta, it, irnd, machep, negep, n, i, steps, nsig, neig, m; double *wptr[10], *ritz, *bnd; SVDRec R = NULL; ierr = 0; // reset the global error flag svdResetCounters(); m = svd_imin(A->rows, A->cols); if (dimensions <= 0 || dimensions > m) dimensions = m; if (iterations <= 0 || iterations > m) iterations = m; if (iterations < dimensions) iterations = dimensions; /* Write output header */ if (SVDVerbosity > 0) write_header(iterations, dimensions, end[0], end[1], TRUE, kappa, A->rows, A->cols, A->vals); /* Check parameters */ if (check_parameters(A, dimensions, iterations, end[0], end[1], TRUE)) return NULL; /* If A is wide, the SVD is computed on its transpose for speed. */ if (A->cols >= A->rows * 1.2) { if (SVDVerbosity > 0) printf("TRANSPOSING THE MATRIX FOR SPEED\n"); transpose = TRUE; A = svdTransposeS(A); } n = A->cols; /* Compute machine precision */ machar(&ibeta, &it, &irnd, &machep, &negep); eps1 = eps * sqrt((double) n); reps = sqrt(eps); eps34 = reps * sqrt(reps); /* Allocate temporary space. */ if (!(wptr[0] = svd_doubleArray(n, TRUE, "las2: wptr[0]"))) goto abort; if (!(wptr[1] = svd_doubleArray(n, FALSE, "las2: wptr[1]"))) goto abort; if (!(wptr[2] = svd_doubleArray(n, FALSE, "las2: wptr[2]"))) goto abort; if (!(wptr[3] = svd_doubleArray(n, FALSE, "las2: wptr[3]"))) goto abort; if (!(wptr[4] = svd_doubleArray(n, FALSE, "las2: wptr[4]"))) goto abort; if (!(wptr[5] = svd_doubleArray(n, FALSE, "las2: wptr[5]"))) goto abort; if (!(wptr[6] = svd_doubleArray(iterations, FALSE, "las2: wptr[6]"))) goto abort; if (!(wptr[7] = svd_doubleArray(iterations, FALSE, "las2: wptr[7]"))) goto abort; if (!(wptr[8] = svd_doubleArray(iterations, FALSE, "las2: wptr[8]"))) goto abort; if (!(wptr[9] = svd_doubleArray(iterations + 1, FALSE, "las2: wptr[9]"))) goto abort; /* Calloc may be unnecessary: */ if (!(ritz = svd_doubleArray(iterations + 1, TRUE, "las2: ritz"))) goto abort; /* Calloc may be unnecessary: */ if (!(bnd = svd_doubleArray(iterations + 1, TRUE, "las2: bnd"))) goto abort; memset(bnd, 127, (iterations + 1) * sizeof(double)); if (!(LanStore = (double **) calloc(iterations + MAXLL, sizeof(double *)))) goto abort; if (!(OPBTemp = svd_doubleArray(A->rows, FALSE, "las2: OPBTemp"))) goto abort; /* Actually run the lanczos thing: */ steps = lanso(A, iterations, dimensions, end[0], end[1], ritz, bnd, wptr, &neig, n); /* Print some stuff. */ if (SVDVerbosity > 0) { printf("NUMBER OF LANCZOS STEPS = %6ld\n" "RITZ VALUES STABILIZED = %6ld\n", steps + 1, neig); } if (SVDVerbosity > 2) { printf("\nCOMPUTED RITZ VALUES (ERROR BNDS)\n"); for (i = 0; i <= steps; i++) printf("%3ld %22.14E (%11.2E)\n", i + 1, ritz[i], bnd[i]); } SAFE_FREE(wptr[0]); SAFE_FREE(wptr[1]); SAFE_FREE(wptr[2]); SAFE_FREE(wptr[3]); SAFE_FREE(wptr[4]); SAFE_FREE(wptr[7]); SAFE_FREE(wptr[8]); /* Compute eigenvectors */ kappa = svd_dmax(fabs(kappa), eps34); R = svdNewSVDRec(); if (!R) { svd_error("svdLAS2: allocation of R failed"); goto cleanup; } R->d = /*svd_imin(nsig, dimensions)*/dimensions; R->Ut = svdNewDMat(R->d, A->rows); R->S = svd_doubleArray(R->d, TRUE, "las2: R->s"); R->Vt = svdNewDMat(R->d, A->cols); if (!R->Ut || !R->S || !R->Vt) { svd_error("svdLAS2: allocation of R failed"); goto cleanup; } nsig = ritvec(n, A, R, kappa, ritz, bnd, wptr[6], wptr[9], wptr[5], steps, neig); if (SVDVerbosity > 1) { printf("\nSINGULAR VALUES: "); svdWriteDenseArray(R->S, R->d, "-", FALSE); if (SVDVerbosity > 2) { printf("\nLEFT SINGULAR VECTORS (transpose of U): "); svdWriteDenseMatrix(R->Ut, "-", SVD_F_DT); printf("\nRIGHT SINGULAR VECTORS (transpose of V): "); svdWriteDenseMatrix(R->Vt, "-", SVD_F_DT); } } if (SVDVerbosity > 0) { printf("SINGULAR VALUES FOUND = %6d\n" "SIGNIFICANT VALUES = %6ld\n", R->d, nsig); } cleanup: for (i = 0; i <= 9; i++) SAFE_FREE(wptr[i]); SAFE_FREE(ritz); SAFE_FREE(bnd); if (LanStore) { for (i = 0; i < iterations + MAXLL; i++) SAFE_FREE(LanStore[i]); SAFE_FREE(LanStore); } SAFE_FREE(OPBTemp); /* This swaps and transposes the singular matrices if A was transposed. */ if (R && transpose) { DMat T; svdFreeSMat(A); T = R->Ut; R->Ut = R->Vt; R->Vt = T; } return R; abort: svd_error("svdLAS2: fatal error, aborting"); return NULL; } /*********************************************************************** * * * ritvec() * * Function computes the singular vectors of matrix A * * * ***********************************************************************/ /*********************************************************************** Description ----------- This function is invoked by landr() only if eigenvectors of the A'A eigenproblem are desired. When called, ritvec() computes the singular vectors of A and writes the result to an unformatted file. Parameters ---------- (input) nrow number of rows of A steps number of Lanczos iterations performed fp_out2 pointer to unformatted output file n dimension of matrix A kappa relative accuracy of ritz values acceptable as eigenvalues of A'A ritz array of ritz values bnd array of error bounds alf array of diagonal elements of the tridiagonal matrix T bet array of off-diagonal elements of T w1, w2 work space (output) xv1 array of eigenvectors of A'A (right singular vectors of A) ierr error code 0 for normal return from imtql2() k if convergence did not occur for k-th eigenvalue in imtql2() nsig number of accepted ritz values based on kappa (local) s work array which is initialized to the identity matrix of order (j + 1) upon calling imtql2(). After the call, s contains the orthonormal eigenvectors of the symmetric tridiagonal matrix T Functions used -------------- BLAS svd_dscal, svd_dcopy, svd_daxpy USER store imtql2 ***********************************************************************/ void rotateArray(double *a, int size, int x) { int i, j, n, start; double t1, t2; if (x == 0) return; j = start = 0; t1 = a[0]; for (i = 0; i < size; i++) { n = (j >= x) ? j - x : j + size - x; t2 = a[n]; a[n] = t1; t1 = t2; j = n; if (j == start) { start = ++j; t1 = a[j]; } } } long ritvec(long n, SMat A, SVDRec R, double kappa, double *ritz, double *bnd, double *alf, double *bet, double *w2, long steps, long neig) { long js, jsq, i, k, /*size,*/ id2, tmp, nsig, x; double *s, *xv2, tmp0, tmp1, xnorm, *w1 = R->Vt->value[0]; js = steps + 1; jsq = js * js; /*size = sizeof(double) * n;*/ s = svd_doubleArray(jsq, TRUE, "ritvec: s"); xv2 = svd_doubleArray(n, FALSE, "ritvec: xv2"); /* initialize s to an identity matrix */ for (i = 0; i < jsq; i+= (js+1)) s[i] = 1.0; svd_dcopy(js, alf, 1, w1, -1); svd_dcopy(steps, &bet[1], 1, &w2[1], -1); /* on return from imtql2(), w1 contains eigenvalues in ascending * order and s contains the corresponding eigenvectors */ imtql2(js, js, w1, w2, s); /*fwrite((char *)&n, sizeof(n), 1, fp_out2); fwrite((char *)&js, sizeof(js), 1, fp_out2); fwrite((char *)&kappa, sizeof(kappa), 1, fp_out2);*/ /*id = 0;*/ nsig = 0; if (ierr) { R->d = 0; } else { x = 0; id2 = jsq - js; for (k = 0; k < js; k++) { tmp = id2; if (bnd[k] <= kappa * fabs(ritz[k]) && k > js-neig-1) { if (--x < 0) x = R->d - 1; w1 = R->Vt->value[x]; for (i = 0; i < n; i++) w1[i] = 0.0; for (i = 0; i < js; i++) { store(n, RETRQ, i, w2); svd_daxpy(n, s[tmp], w2, 1, w1, 1); tmp -= js; } /*fwrite((char *)w1, size, 1, fp_out2);*/ /* store the w1 vector row-wise in array xv1; * size of xv1 is (steps+1) * (nrow+ncol) elements * and each vector, even though only ncol long, * will have (nrow+ncol) elements in xv1. * It is as if xv1 is a 2-d array (steps+1) by * (nrow+ncol) and each vector occupies a row */ /* j is the index in the R arrays, which are sorted by high to low singular values. */ /*for (i = 0; i < n; i++) R->Vt->value[x]xv1[id++] = w1[i];*/ /*id += nrow;*/ nsig++; } id2++; } SAFE_FREE(s); /* Rotate the singular vectors and values. */ /* x is now the location of the highest singular value. */ rotateArray(R->Vt->value[0], R->Vt->rows * R->Vt->cols, x * R->Vt->cols); R->d = svd_imin(R->d, nsig); for (x = 0; x < R->d; x++) { /* multiply by matrix B first */ svd_opb(A, R->Vt->value[x], xv2, OPBTemp); tmp0 = svd_ddot(n, R->Vt->value[x], 1, xv2, 1); svd_daxpy(n, -tmp0, R->Vt->value[x], 1, xv2, 1); tmp0 = sqrt(tmp0); xnorm = sqrt(svd_ddot(n, xv2, 1, xv2, 1)); /* multiply by matrix A to get (scaled) left s-vector */ svd_opa(A, R->Vt->value[x], R->Ut->value[x]); tmp1 = 1.0 / tmp0; svd_dscal(A->rows, tmp1, R->Ut->value[x], 1); xnorm *= tmp1; bnd[i] = xnorm; R->S[x] = tmp0; } } SAFE_FREE(s); SAFE_FREE(xv2); return nsig; } /*********************************************************************** * * * lanso() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function determines when the restart of the Lanczos algorithm should occur and when it should terminate. Arguments --------- (input) n dimension of the eigenproblem for matrix B iterations upper limit of desired number of lanczos steps dimensions upper limit of desired number of eigenpairs endl left end of interval containing unwanted eigenvalues endr right end of interval containing unwanted eigenvalues ritz array to hold the ritz values bnd array to hold the error bounds wptr array of pointers that point to work space: wptr[0]-wptr[5] six vectors of length n wptr[6] array to hold diagonal of the tridiagonal matrix T wptr[9] array to hold off-diagonal of T wptr[7] orthogonality estimate of Lanczos vectors at step j wptr[8] orthogonality estimate of Lanczos vectors at step j-1 (output) j number of Lanczos steps actually taken neig number of ritz values stabilized ritz array to hold the ritz values bnd array to hold the error bounds ierr (globally declared) error flag ierr = 8192 if stpone() fails to find a starting vector ierr = k if convergence did not occur for k-th eigenvalue in imtqlb() ierr = 0 otherwise Functions used -------------- LAS stpone, error_bound, lanczos_step MISC svd_dsort2 UTILITY svd_imin, svd_imax ***********************************************************************/ int lanso(SMat A, long iterations, long dimensions, double endl, double endr, double *ritz, double *bnd, double *wptr[], long *neigp, long n) { double *alf, *eta, *oldeta, *bet, *wrk, rnm, tol; long ll, first, last, ENOUGH, id2, id3, i, l, neig, j = 0, intro = 0; alf = wptr[6]; eta = wptr[7]; oldeta = wptr[8]; bet = wptr[9]; wrk = wptr[5]; /* take the first step */ stpone(A, wptr, &rnm, &tol, n); if (!rnm || ierr) return 0; eta[0] = eps1; oldeta[0] = eps1; ll = 0; first = 1; last = svd_imin(dimensions + svd_imax(8, dimensions), iterations); ENOUGH = FALSE; /*id1 = 0;*/ int nIterations=0; while (/*id1 < dimensions && */!ENOUGH) { if (rnm <= tol) rnm = 0.0; /* the actual lanczos loop */ j = lanczos_step(A, first, last, wptr, alf, eta, oldeta, bet, &ll, &ENOUGH, &rnm, &tol, n); nIterations++; if (ENOUGH) j = j - 1; else j = last - 1; first = j + 1; bet[j+1] = rnm; /* analyze T */ l = 0; for (id2 = 0; id2 < j; id2++) { if (l > j) break; for (i = l; i <= j; i++) if (!bet[i+1]) break; if (i > j) i = j; /* now i is at the end of an unreduced submatrix */ svd_dcopy(i-l+1, &alf[l], 1, &ritz[l], -1); svd_dcopy(i-l, &bet[l+1], 1, &wrk[l+1], -1); imtqlb(i-l+1, &ritz[l], &wrk[l], &bnd[l]); if (ierr) { svd_error("svdLAS2: imtqlb failed to converge (ierr = %ld)\n", ierr); svd_error(" l = %ld i = %ld\n", l, i); for (id3 = l; id3 <= i; id3++) svd_error(" %ld %lg %lg %lg\n", id3, ritz[id3], wrk[id3], bnd[id3]); } for (id3 = l; id3 <= i; id3++) bnd[id3] = rnm * fabs(bnd[id3]); l = i + 1; } /* sort eigenvalues into increasing order */ svd_dsort2((j+1) / 2, j + 1, ritz, bnd); /* for (i = 0; i < iterations; i++) printf("%f ", ritz[i]); printf("\n"); */ /* massage error bounds for very close ritz values */ neig = error_bound(&ENOUGH, endl, endr, ritz, bnd, j, tol); *neigp = neig; /* should we stop? */ if (neig < dimensions) { if (!neig) { last = first + 9; intro = first; } else last = first + svd_imax(3, 1 + ((j - intro) * (dimensions-neig)) / neig); last = svd_imin(last, iterations); } else ENOUGH = TRUE; ENOUGH = ENOUGH || first >= iterations; /* id1++; */ /* printf("id1=%d dimen=%d first=%d\n", id1, dimensions, first); */ } printf("niterations %d\n",nIterations); store(n, STORQ, j, wptr[1]); return j; } /*********************************************************************** * * * lanczos_step() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function embodies a single Lanczos step Arguments --------- (input) n dimension of the eigenproblem for matrix B first start of index through loop last end of index through loop wptr array of pointers pointing to work space alf array to hold diagonal of the tridiagonal matrix T eta orthogonality estimate of Lanczos vectors at step j oldeta orthogonality estimate of Lanczos vectors at step j-1 bet array to hold off-diagonal of T ll number of intitial Lanczos vectors in local orthog. (has value of 0, 1 or 2) enough stop flag Functions used -------------- BLAS svd_ddot, svd_dscal, svd_daxpy, svd_datx, svd_dcopy USER store LAS purge, ortbnd, startv UTILITY svd_imin, svd_imax ***********************************************************************/ long lanczos_step(SMat A, long first, long last, double *wptr[], double *alf, double *eta, double *oldeta, double *bet, long *ll, long *enough, double *rnmp, double *tolp, long n) { double t, *mid, rnm = *rnmp, tol = *tolp, anorm; long i, j; for (j=first; j 4.0 * fabs(alf[j]))) *ll = j; for (i=0; i < svd_imin(*ll, j-1); i++) { store(n, RETRP, i, wptr[5]); t = svd_ddot(n, wptr[5], 1, wptr[0], 1); store(n, RETRQ, i, wptr[5]); svd_daxpy(n, -t, wptr[5], 1, wptr[0], 1); eta[i] = eps1; oldeta[i] = eps1; } /* extended local reorthogonalization */ t = svd_ddot(n, wptr[0], 1, wptr[4], 1); svd_daxpy(n, -t, wptr[2], 1, wptr[0], 1); if (bet[j] > 0.0) bet[j] = bet[j] + t; t = svd_ddot(n, wptr[0], 1, wptr[3], 1); svd_daxpy(n, -t, wptr[1], 1, wptr[0], 1); alf[j] = alf[j] + t; svd_dcopy(n, wptr[0], 1, wptr[4], 1); rnm = sqrt(svd_ddot(n, wptr[0], 1, wptr[4], 1)); anorm = bet[j] + fabs(alf[j]) + rnm; tol = reps * anorm; /* update the orthogonality bounds */ ortbnd(alf, eta, oldeta, bet, j, rnm); /* restore the orthogonality state when needed */ purge(n, *ll, wptr[0], wptr[1], wptr[4], wptr[3], wptr[5], eta, oldeta, j, &rnm, tol); if (rnm <= tol) rnm = 0.0; } *rnmp = rnm; *tolp = tol; return j; } /*********************************************************************** * * * ortbnd() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Funtion updates the eta recurrence Arguments --------- (input) alf array to hold diagonal of the tridiagonal matrix T eta orthogonality estimate of Lanczos vectors at step j oldeta orthogonality estimate of Lanczos vectors at step j-1 bet array to hold off-diagonal of T n dimension of the eigenproblem for matrix B j dimension of T rnm norm of the next residual vector eps1 roundoff estimate for dot product of two unit vectors (output) eta orthogonality estimate of Lanczos vectors at step j+1 oldeta orthogonality estimate of Lanczos vectors at step j Functions used -------------- BLAS svd_dswap ***********************************************************************/ void ortbnd(double *alf, double *eta, double *oldeta, double *bet, long step, double rnm) { long i; if (step < 1) return; if (rnm) { if (step > 1) { oldeta[0] = (bet[1] * eta[1] + (alf[0]-alf[step]) * eta[0] - bet[step] * oldeta[0]) / rnm + eps1; } for (i=1; i<=step-2; i++) oldeta[i] = (bet[i+1] * eta[i+1] + (alf[i]-alf[step]) * eta[i] + bet[i] * eta[i-1] - bet[step] * oldeta[i])/rnm + eps1; } oldeta[step-1] = eps1; svd_dswap(step, oldeta, 1, eta, 1); eta[step] = eps1; return; } /*********************************************************************** * * * purge() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function examines the state of orthogonality between the new Lanczos vector and the previous ones to decide whether re-orthogonalization should be performed Arguments --------- (input) n dimension of the eigenproblem for matrix B ll number of intitial Lanczos vectors in local orthog. r residual vector to become next Lanczos vector q current Lanczos vector ra previous Lanczos vector qa previous Lanczos vector wrk temporary vector to hold the previous Lanczos vector eta state of orthogonality between r and prev. Lanczos vectors oldeta state of orthogonality between q and prev. Lanczos vectors j current Lanczos step (output) r residual vector orthogonalized against previous Lanczos vectors q current Lanczos vector orthogonalized against previous ones Functions used -------------- BLAS svd_daxpy, svd_dcopy, svd_idamax, svd_ddot USER store ***********************************************************************/ void purge(long n, long ll, double *r, double *q, double *ra, double *qa, double *wrk, double *eta, double *oldeta, long step, double *rnmp, double tol) { double t, tq, tr, reps1, rnm = *rnmp; long k, iteration, flag, i; if (step < ll+2) return; k = svd_idamax(step - (ll+1), &eta[ll], 1) + ll; if (fabs(eta[k]) > reps) { reps1 = eps1 / reps; iteration = 0; flag = TRUE; while (iteration < 2 && flag) { if (rnm > tol) { /* bring in a lanczos vector t and orthogonalize both * r and q against it */ tq = 0.0; tr = 0.0; for (i = ll; i < step; i++) { store(n, RETRQ, i, wrk); t = -svd_ddot(n, qa, 1, wrk, 1); tq += fabs(t); svd_daxpy(n, t, wrk, 1, q, 1); t = -svd_ddot(n, ra, 1, wrk, 1); tr += fabs(t); svd_daxpy(n, t, wrk, 1, r, 1); } svd_dcopy(n, q, 1, qa, 1); t = -svd_ddot(n, r, 1, qa, 1); tr += fabs(t); svd_daxpy(n, t, q, 1, r, 1); svd_dcopy(n, r, 1, ra, 1); rnm = sqrt(svd_ddot(n, ra, 1, r, 1)); if (tq <= reps1 && tr <= reps1 * rnm) flag = FALSE; } iteration++; } for (i = ll; i <= step; i++) { eta[i] = eps1; oldeta[i] = eps1; } } *rnmp = rnm; return; } /*********************************************************************** * * * stpone() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function performs the first step of the Lanczos algorithm. It also does a step of extended local re-orthogonalization. Arguments --------- (input) n dimension of the eigenproblem for matrix B (output) ierr error flag wptr array of pointers that point to work space that contains wptr[0] r[j] wptr[1] q[j] wptr[2] q[j-1] wptr[3] p wptr[4] p[j-1] wptr[6] diagonal elements of matrix T Functions used -------------- BLAS svd_daxpy, svd_datx, svd_dcopy, svd_ddot, svd_dscal USER store, opb LAS startv ***********************************************************************/ void stpone(SMat A, double *wrkptr[], double *rnmp, double *tolp, long n) { double t, *alf, rnm, anorm; alf = wrkptr[6]; /* get initial vector; default is random */ rnm = startv(A, wrkptr, 0, n); if (rnm == 0.0 || ierr != 0) return; /* normalize starting vector */ t = 1.0 / rnm; svd_datx(n, t, wrkptr[0], 1, wrkptr[1], 1); svd_dscal(n, t, wrkptr[3], 1); /* take the first step */ svd_opb(A, wrkptr[3], wrkptr[0], OPBTemp); alf[0] = svd_ddot(n, wrkptr[0], 1, wrkptr[3], 1); svd_daxpy(n, -alf[0], wrkptr[1], 1, wrkptr[0], 1); t = svd_ddot(n, wrkptr[0], 1, wrkptr[3], 1); svd_daxpy(n, -t, wrkptr[1], 1, wrkptr[0], 1); alf[0] += t; svd_dcopy(n, wrkptr[0], 1, wrkptr[4], 1); rnm = sqrt(svd_ddot(n, wrkptr[0], 1, wrkptr[4], 1)); anorm = rnm + fabs(alf[0]); *rnmp = rnm; *tolp = reps * anorm; return; } /*********************************************************************** * * * startv() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function delivers a starting vector in r and returns |r|; it returns zero if the range is spanned, and ierr is non-zero if no starting vector within range of operator can be found. Parameters --------- (input) n dimension of the eigenproblem matrix B wptr array of pointers that point to work space j starting index for a Lanczos run eps machine epsilon (relative precision) (output) wptr array of pointers that point to work space that contains r[j], q[j], q[j-1], p[j], p[j-1] ierr error flag (nonzero if no starting vector can be found) Functions used -------------- BLAS svd_ddot, svd_dcopy, svd_daxpy USER svd_opb, store MISC random ***********************************************************************/ double startv(SMat A, double *wptr[], long step, long n) { double rnm2, *r, t; long irand; long id, i; /* get initial vector; default is random */ rnm2 = svd_ddot(n, wptr[0], 1, wptr[0], 1); irand = 918273 + step; r = wptr[0]; for (id = 0; id < 3; id++) { if (id > 0 || step > 0 || rnm2 == 0) for (i = 0; i < n; i++) r[i] = svd_random2(&irand); svd_dcopy(n, wptr[0], 1, wptr[3], 1); /* apply operator to put r in range (essential if m singular) */ svd_opb(A, wptr[3], wptr[0], OPBTemp); svd_dcopy(n, wptr[0], 1, wptr[3], 1); rnm2 = svd_ddot(n, wptr[0], 1, wptr[3], 1); if (rnm2 > 0.0) break; } /* fatal error */ if (rnm2 <= 0.0) { ierr = 8192; return(-1); } if (step > 0) { for (i = 0; i < step; i++) { store(n, RETRQ, i, wptr[5]); t = -svd_ddot(n, wptr[3], 1, wptr[5], 1); svd_daxpy(n, t, wptr[5], 1, wptr[0], 1); } /* make sure q[step] is orthogonal to q[step-1] */ t = svd_ddot(n, wptr[4], 1, wptr[0], 1); svd_daxpy(n, -t, wptr[2], 1, wptr[0], 1); svd_dcopy(n, wptr[0], 1, wptr[3], 1); t = svd_ddot(n, wptr[3], 1, wptr[0], 1); if (t <= eps * rnm2) t = 0.0; rnm2 = t; } return(sqrt(rnm2)); } /*********************************************************************** * * * error_bound() * * * ***********************************************************************/ /*********************************************************************** Description ----------- Function massages error bounds for very close ritz values by placing a gap between them. The error bounds are then refined to reflect this. Arguments --------- (input) endl left end of interval containing unwanted eigenvalues endr right end of interval containing unwanted eigenvalues ritz array to store the ritz values bnd array to store the error bounds enough stop flag Functions used -------------- BLAS svd_idamax UTILITY svd_dmin ***********************************************************************/ long error_bound(long *enough, double endl, double endr, double *ritz, double *bnd, long step, double tol) { long mid, i, neig; double gapl, gap; /* massage error bounds for very close ritz values */ mid = svd_idamax(step + 1, bnd, 1); for (i=((step+1) + (step-1)) / 2; i >= mid + 1; i -= 1) if (fabs(ritz[i-1] - ritz[i]) < eps34 * fabs(ritz[i])) if (bnd[i] > tol && bnd[i-1] > tol) { bnd[i-1] = sqrt(bnd[i] * bnd[i] + bnd[i-1] * bnd[i-1]); bnd[i] = 0.0; } for (i=((step+1) - (step-1)) / 2; i <= mid - 1; i +=1 ) if (fabs(ritz[i+1] - ritz[i]) < eps34 * fabs(ritz[i])) if (bnd[i] > tol && bnd[i+1] > tol) { bnd[i+1] = sqrt(bnd[i] * bnd[i] + bnd[i+1] * bnd[i+1]); bnd[i] = 0.0; } /* refine the error bounds */ neig = 0; gapl = ritz[step] - ritz[0]; for (i = 0; i <= step; i++) { gap = gapl; if (i < step) gapl = ritz[i+1] - ritz[i]; gap = svd_dmin(gap, gapl); if (gap > bnd[i]) bnd[i] = bnd[i] * (bnd[i] / gap); if (bnd[i] <= 16.0 * eps * fabs(ritz[i])) { neig++; if (!*enough) *enough = endl < ritz[i] && ritz[i] < endr; } } return neig; } /*********************************************************************** * * * imtqlb() * * * ***********************************************************************/ /*********************************************************************** Description ----------- imtqlb() is a translation of a Fortran version of the Algol procedure IMTQL1, Num. Math. 12, 377-383(1968) by Martin and Wilkinson, as modified in Num. Math. 15, 450(1970) by Dubrulle. Handbook for Auto. Comp., vol.II-Linear Algebra, 241-248(1971). See also B. T. Smith et al, Eispack Guide, Lecture Notes in Computer Science, Springer-Verlag, (1976). The function finds the eigenvalues of a symmetric tridiagonal matrix by the implicit QL method. Arguments --------- (input) n order of the symmetric tridiagonal matrix d contains the diagonal elements of the input matrix e contains the subdiagonal elements of the input matrix in its last n-1 positions. e[0] is arbitrary (output) d contains the eigenvalues in ascending order. if an error exit is made, the eigenvalues are correct and ordered for indices 0,1,...ierr, but may not be the smallest eigenvalues. e has been destroyed. ierr set to zero for normal return, j if the j-th eigenvalue has not been determined after 30 iterations. Functions used -------------- UTILITY svd_fsign MISC svd_pythag ***********************************************************************/ void imtqlb(long n, double d[], double e[], double bnd[]) { long last, l, m, i, iteration; /* various flags */ long exchange, convergence, underflow; double b, test, g, r, s, c, p, f; if (n == 1) return; ierr = 0; bnd[0] = 1.0; last = n - 1; for (i = 1; i < n; i++) { bnd[i] = 0.0; e[i-1] = e[i]; } e[last] = 0.0; for (l = 0; l < n; l++) { iteration = 0; while (iteration <= 30) { for (m = l; m < n; m++) { convergence = FALSE; if (m == last) break; else { test = fabs(d[m]) + fabs(d[m+1]); if (test + fabs(e[m]) == test) convergence = TRUE; } if (convergence) break; } p = d[l]; f = bnd[l]; if (m != l) { if (iteration == 30) { ierr = l; return; } iteration += 1; /*........ form shift ........*/ g = (d[l+1] - p) / (2.0 * e[l]); r = svd_pythag(g, 1.0); g = d[m] - p + e[l] / (g + svd_fsign(r, g)); s = 1.0; c = 1.0; p = 0.0; underflow = FALSE; i = m - 1; while (underflow == FALSE && i >= l) { f = s * e[i]; b = c * e[i]; r = svd_pythag(f, g); e[i+1] = r; if (r == 0.0) underflow = TRUE; else { s = f / r; c = g / r; g = d[i+1] - p; r = (d[i] - g) * s + 2.0 * c * b; p = s * r; d[i+1] = g + p; g = c * r - b; f = bnd[i+1]; bnd[i+1] = s * bnd[i] + c * f; bnd[i] = c * bnd[i] - s * f; i--; } } /* end while (underflow != FALSE && i >= l) */ /*........ recover from underflow .........*/ if (underflow) { d[i+1] -= p; e[m] = 0.0; } else { d[l] -= p; e[l] = g; e[m] = 0.0; } } /* end if (m != l) */ else { /* order the eigenvalues */ exchange = TRUE; if (l != 0) { i = l; while (i >= 1 && exchange == TRUE) { if (p < d[i-1]) { d[i] = d[i-1]; bnd[i] = bnd[i-1]; i--; } else exchange = FALSE; } } if (exchange) i = 0; d[i] = p; bnd[i] = f; iteration = 31; } } /* end while (iteration <= 30) */ } /* end for (l=0; l= l) { f = s * e[i]; b = c * e[i]; r = svd_pythag(f, g); e[i+1] = r; if (r == 0.0) underflow = TRUE; else { s = f / r; c = g / r; g = d[i+1] - p; r = (d[i] - g) * s + 2.0 * c * b; p = s * r; d[i+1] = g + p; g = c * r - b; /* form vector */ for (k = 0; k < nnm; k += n) { index = k + i; f = z[index+1]; z[index+1] = s * z[index] + c * f; z[index] = c * z[index] - s * f; } i--; } } /* end while (underflow != FALSE && i >= l) */ /*........ recover from underflow .........*/ if (underflow) { d[i+1] -= p; e[m] = 0.0; } else { d[l] -= p; e[l] = g; e[m] = 0.0; } } else break; } /*...... end while (iteration <= 30) .........*/ } /*...... end for (l=0; l= MAXLL) { svd_error("svdLAS2: store (STORP) called with j >= MAXLL"); break; } if (!LanStore[j]) { if (!(LanStore[j] = svd_doubleArray(n, FALSE, "LanStore[j]"))) svd_fatalError("svdLAS2: failed to allocate LanStore[%d]", j); } svd_dcopy(n, s, 1, LanStore[j], 1); break; case RETRP: if (j >= MAXLL) { svd_error("svdLAS2: store (RETRP) called with j >= MAXLL"); break; } if (!LanStore[j]) svd_fatalError("svdLAS2: store (RETRP) called on index %d (not allocated)", j); svd_dcopy(n, LanStore[j], 1, s, 1); break; } return; } ================================================ FILE: GSLAM/main.cpp ================================================ // // main.cpp // STAR // // Created by Chaos on 4/25/16. // Copyright © 2016 Chaos. All rights reserved. // #include "opencv2/imgproc/imgproc.hpp" #import #import #import #include "System.h" #include "GlobalReconstruction.h" #include #include int main(int argc, char **argv){ printf("version no global ptr\n"); system("exec rm -r /Users/chaos/Desktop/debug/*"); argv[1]="/Users/chaos/Downloads/sequences/robot"; argv[2]="/Users/chaos/Downloads/sequences/ORBvoc.txt"; std::cout<<"process "< yuv(3); cv::split(uv,&yuv[1]); cv::resize(image,yuv[0],cv::Size(width/2,height/2)); cv::Mat rgb; cv::merge((const cv::Mat*)(&yuv[0]),3,rgb); cv::cvtColor(rgb,rgb,CV_YCrCb2RGB); double timestamp; timestamps>>timestamp; if(frameCount==slamSystem.frameStart){ slamSystem.colorImage=&rgb; image.copyTo(curImage); }else if(frameCount>slamSystem.frameStart){ image.copyTo(nextImage); slamSystem.preloadImage=&nextImage; std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); GSLAM::Transform pose=slamSystem.Track(curImage,timestamp,frameCount); std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); double ttrack= std::chrono::duration_cast >(t2 - t1).count(); record<slamSystem.frameEnd){ break; } } record.close(); slamSystem.finish(); cout<<"processed "< #include "opencv2/core/core.hpp" //#include "opencv2/video/tracking.hpp" //void computePyramid(KLT_TrackingContext tc, // const cv::Mat &img, // std::vector &pyramid){ // // if (pyramid.size()!=3*tc->nPyramidLevels) { // pyramid.resize(3*tc->nPyramidLevels); // } // // int ncols = img.cols, nrows = img.rows; // float pyramid_sigma = tc->subsampling * tc->pyramid_sigma_fact; /* empirically determined */ // // cv::Mat tmpimg; // img.convertTo(tmpimg,CV_32F); // // computeSmoothedImage(tmpimg,pyramid[0],_KLTComputeSmoothSigma(tc)); // computeGradients(pyramid[0],pyramid[1],pyramid[2],tc->grad_sigma); // for (int i = 1 ; i < tc->nPyramidLevels ; i++) { // computeSmoothedImage(pyramid[3*i-3],tmpimg,pyramid_sigma); // ncols /= tc->subsampling; nrows /= tc->subsampling; // cv::resize(tmpimg,pyramid[3*i],cv::Size(ncols,nrows)); // computeGradients(pyramid[3*i],pyramid[3*i+1],pyramid[3*i+2],tc->grad_sigma); // } //} #include #include "KLTUtil.h" inline void computeGxGy(const cv::Mat& img,cv::Mat& grad,cv::Mat& filter1,cv::Mat& filter2){ cv::sepFilter2D(img,grad,CV_32F,filter1,filter2); } void computePyramid(KLT_TrackingContext tc, const cv::Mat &img, std::vector &pyramid){ if (pyramid.size()!=3*tc->nPyramidLevels) { pyramid.resize(3*tc->nPyramidLevels); } int ncols = img.cols, nrows = img.rows; float pyramid_sigma = tc->subsampling * tc->pyramid_sigma_fact; /* empirically determined */ cv::Mat tmpimg; img.convertTo(tmpimg,CV_32F); computeSmoothedImage(tmpimg,pyramid[0],_KLTComputeSmoothSigma(tc)); for (int i = 1 ; i < tc->nPyramidLevels ; i++) { computeSmoothedImage(pyramid[3*i-3],tmpimg,pyramid_sigma); ncols /= tc->subsampling; nrows /= tc->subsampling; cv::resize(tmpimg,pyramid[3*i],cv::Size(ncols,nrows)); } if (fabs(tc->grad_sigma - sigma_last) > 0.05){ computeKernels(tc->grad_sigma,&gauss_kernel, &gaussderiv_kernel); } cv::Mat kernelGauss(1,gauss_kernel.width,CV_32F,gauss_kernel.data); cv::Mat kernelGaussDeriv(1,gaussderiv_kernel.width,CV_32F,gaussderiv_kernel.data); std::vector threads(2*tc->nPyramidLevels); for (int i = 0 ; i < tc->nPyramidLevels ; i++) { threads[2*i]=std::thread(computeGxGy,std::ref(pyramid[3*i]),std::ref(pyramid[3*i+1]),std::ref(kernelGaussDeriv),std::ref(kernelGauss)); threads[2*i+1]=std::thread(computeGxGy,std::ref(pyramid[3*i]),std::ref(pyramid[3*i+2]),std::ref(kernelGauss),std::ref(kernelGaussDeriv)); } for (int i=0;i &pyramid); ================================================ FILE: GSLAM/random_generators.hpp ================================================ /****************************************************************************** * Author: Laurent Kneip * * Contact: kneip.laurent@gmail.com * * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. * * * * Redistribution and use in source and binary forms, with or without * * modification, are permitted provided that the following conditions * * are met: * * * Redistributions of source code must retain the above copyright * * notice, this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright * * notice, this list of conditions and the following disclaimer in the * * documentation and/or other materials provided with the distribution. * * * Neither the name of ANU nor the names of its contributors may be * * used to endorse or promote products derived from this software without * * specific prior written permission. * * * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * * SUCH DAMAGE. * ******************************************************************************/ #ifndef OPENGV_RANDOM_GENERATORS_HPP_ #define OPENGV_RANDOM_GENERATORS_HPP_ #include #include #include namespace opengv { void initializeRandomSeed(); Eigen::Vector3d generateRandomPoint( double maximumDepth, double minimumDepth ); Eigen::Vector3d generateRandomPointPlane(); Eigen::Vector3d addNoise( double noiseLevel, Eigen::Vector3d cleanPoint ); Eigen::Vector3d generateRandomTranslation( double maximumParallax ); Eigen::Vector3d generateRandomDirectionTranslation( double parallax ); Eigen::Matrix3d generateRandomRotation( double maxAngle ); Eigen::Matrix3d generateRandomRotation(); } #endif /* OPENGV_RANDOM_GENERATORS_HPP_ */ ================================================ FILE: GSLAM/selectGoodFeatures.hpp ================================================ // // selectGoodFeatures.hpp // STARCK // // Created by Chaos on 3/30/16. // Copyright © 2016 Chaos. All rights reserved. // /********************************************************************* * selectGoodFeatures.c * *********************************************************************/ /* Standard includes */ #include #include /* malloc(), qsort() */ #include /* fflush() */ #include /* memset() */ #include "opencv2/core/core.hpp" #include "KLT.h" #pragma once #define KLT_NOT_FOUND -1 //#define USE_DILATE cv::Size gridSize(20,20); cv::Size sizeByGrid; int minFeatureInGrid; typedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode; static void _fillFeaturemap(int x, int y, uchar *featuremap, int mindist, int ncols, int nrows){ int ix, iy; for (iy = y - mindist ; iy <= y + mindist ; iy++){ for (ix = x - mindist ; ix <= x + mindist ; ix++) if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows) featuremap[iy*ncols+ix] = 1; } } /********************************************************************* * _enforceMinimumDistance * * Removes features that are within close proximity to better features. * * INPUTS * featurelist: A list of features. The nFeatures property * is used. * * OUTPUTS * featurelist: Is overwritten. Nearby "redundant" features are removed. * Writes -1's into the remaining elements. * * RETURNS * The number of remaining features. */ static int _comparePoints(const void *a, const void *b) { int v1 = *(((int *) a) + 2); int v2 = *(((int *) b) + 2); if (v1 > v2) return(-1); else if (v1 < v2) return(1); else return(0); } static void _sortPointList( int *pointlist, int npoints){ qsort(pointlist, npoints, 3*sizeof(int), _comparePoints); } static void _enforceMinimumDistance(uchar *featuremap, int *pointlist, /* featurepoints */ int npoints, /* number of featurepoints */ KLT_FeatureList featurelist, /* features */ int ncols, int nrows, /* size of images */ int mindist, /* min. dist b/w features */ int min_eigenvalue, /* min. eigenvalue */ bool overwriteAllFeatures) { int indx; /* Index into features */ int x, y, val; /* Location and trackability of pixel under consideration */ //int *featuremap; /* Boolean array recording proximity of features */ int *ptr; /* Cannot add features with an eigenvalue less than one */ if (min_eigenvalue < 1) min_eigenvalue = 1; /* Allocate memory for feature map and clear it */ //featuremap = (int *) malloc(ncols * nrows * sizeof(int)); //memset(featuremap, 0, ncols*nrows*sizeof(int)); /* Necessary because code below works with (mindist-1) */ mindist--; /* If we are keeping all old good features, then add them to the featuremap */ /*if (!overwriteAllFeatures) for (indx = 0 ; indx < featurelist->nFeatures ; indx++) if (featurelist->feature[indx]->val >= 0) { x = (int) featurelist->feature[indx]->x; y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y,INT_MAX,featuremap, mindist, ncols, nrows); } */ /* For each feature point, in descending order of importance, do ... */ ptr = pointlist; indx = 0; while (1) { /* If we can't add all the points, then fill in the rest of the featurelist with -1's */ if (ptr >= pointlist + 3*npoints) { while (indx < featurelist->nFeatures) { if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) { featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = KLT_NOT_FOUND; /* featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0;*/ } indx++; } break; } x = *ptr++; y = *ptr++; val = *ptr++; /* Ensure that feature is in-bounds */ assert(x >= 0); assert(x < ncols); assert(y >= 0); assert(y < nrows); while (!overwriteAllFeatures && indx < featurelist->nFeatures && featurelist->feature[indx]->val >= 0) indx++; if (indx >= featurelist->nFeatures) break; /* If no neighbor has been selected, and if the minimum eigenvalue is large enough, then add feature to the current list */ if (!(featuremap[y*ncols+x]) && val >= min_eigenvalue) { featurelist->feature[indx]->x = (float) x; featurelist->feature[indx]->y = (float) y; featurelist->feature[indx]->val = (int) val; /* featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0;*/ indx++; /* Fill in surrounding region of feature map, but make sure that pixels are in-bounds */ _fillFeaturemap(x, y,featuremap, mindist, ncols, nrows); } } /* Free feature map */ //free(featuremap); } /********************************************************************* * _sortPointList */ /********************************************************************* * _minEigenvalue * * Given the three distinct elements of the symmetric 2x2 matrix * [gxx gxy] * [gxy gyy], * Returns the minimum eigenvalue of the matrix. */ static float _minEigenvalue(float gxx, float gxy, float gyy) { return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f); } struct greaterThanPtr : public std::binary_function { bool operator () (const float * a, const float * b) const { return *a > *b; } }; /*********************************************************************/ void _KLTSelectGoodFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList featurelist, selectionMode mode) { int npoints = 0; bool overwriteAllFeatures = (mode == SELECTING_ALL) ? true : false; bool floatimages_created = false; int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); if (!overwriteAllFeatures){ for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y,featuremap,tc->mindist, ncols, nrows); } } } #ifdef USE_DILATE cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F); #endif int nSkippedPixels=tc->nSkippedPixels; /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; /* For most of the pixels in the image, do ... */ ptr = pointlist; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; #ifdef USE_DILATE float* eigPtr=(float*)eig.ptr(y); #endif for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]) { continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float val= _minEigenvalue(gxx, gxy, gyy); if (val > limit) { val = (float) limit; } if (val>tc->min_eigenvalue){ #ifdef USE_DILATE eigPtr[x] = val; #else *ptr++ = x; *ptr++ = y; *ptr++ = (int) val; npoints++; #endif } } } } #ifdef USE_DILATE cv::Mat tmp; cv::dilate(eig,tmp,cv::Mat()); cv::Size imgsize=tmp.size(); std::vector tmpCorners; for( int y = 1; y < imgsize.height - 1; y++ ) { const float* eig_data = (const float*)eig.ptr(y); const float* tmp_data = (const float*)tmp.ptr(y); for( int x = 1; x < imgsize.width - 1; x++ ) { float val = eig_data[x]; if( val != 0&& val == tmp_data[x]) tmpCorners.push_back(eig_data + x); } } std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr()); npoints=tmpCorners.size(); for (int i=0;imindist, tc->min_eigenvalue, overwriteAllFeatures); /* Free memory */ free(featuremap); free(pointlist); } void KLTSelectGoodFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL); } void KLTReplaceLostFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME); } typedef enum {SELECTING_ALL2, REPLACING_SOME2} selectionMode2; static void _fillFeaturemap2(int x, int y, uchar *featuremap, int mindist, int ncols, int nrows){ int ix, iy; for (iy = y - mindist ; iy <= y + mindist ; iy++){ for (ix = x - mindist ; ix <= x + mindist ; ix++) if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows) featuremap[iy*ncols+ix] = 1; } } /********************************************************************* * _enforceMinimumDistance * * Removes features that are within close proximity to better features. * * INPUTS * featurelist: A list of features. The nFeatures property * is used. * * OUTPUTS * featurelist: Is overwritten. Nearby "redundant" features are removed. * Writes -1's into the remaining elements. * * RETURNS * The number of remaining features. */ static int _comparePoints2(const void *a, const void *b) { int v1 = *(((int *) a) + 2); int v2 = *(((int *) b) + 2); if (v1 > v2) return(-1); else if (v1 < v2) return(1); else return(0); } static void _sortPointList2( int *pointlist, int npoints){ qsort(pointlist, npoints, 3*sizeof(int), _comparePoints2); } static float _minEigenvalue2(float gxx, float gxy, float gyy) { return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f); } void _enforceMinimumDistance2(uchar* featuremap, int *pointlist, /* featurepoints */ int npoints, /* number of featurepoints */ KLT_FeatureList featurelist, /* features */ int ncols, int nrows, /* size of images */ int mindist, /* min. dist b/w features */ int min_eigenvalue, /* min. eigenvalue */ bool overwriteAllFeatures) { int indx; /* Index into features */ int x, y, val; /* Location and trackability of pixel under consideration */ /* Boolean array recording proximity of features */ int *ptr; /* Cannot add features with an eigenvalue less than one */ if (min_eigenvalue < 1) min_eigenvalue = 1; //mindist--; /* Allocate memory for feature map and clear it */ /* featuremap = (uchar *) malloc(ncols * nrows * sizeof(uchar)); memset(featuremap, 0, ncols*nrows); if (!overwriteAllFeatures) for (indx = 0 ; indx < featurelist->nFeatures ; indx++) if (featurelist->feature[indx]->val >= 0) { x = (int) featurelist->feature[indx]->x; y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows); } */ /* For each feature point, in descending order of importance, do ... */ ptr = pointlist; indx = 0; while (1) { /* If we can't add all the points, then fill in the rest of the featurelist with -1's */ if (ptr >= pointlist + 3*npoints) { while (indx < featurelist->nFeatures) { if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) { featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = KLT_NOT_FOUND; /*featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL;*/ featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; } indx++; } break; } x = *ptr++; y = *ptr++; val = *ptr++; /* Ensure that feature is in-bounds */ assert(x >= 0); assert(x < ncols); assert(y >= 0); assert(y < nrows); while (!overwriteAllFeatures && indx < featurelist->nFeatures && featurelist->feature[indx]->val >= 0) indx++; if (indx >= featurelist->nFeatures) break; /* If no neighbor has been selected, and if the minimum eigenvalue is large enough, then add feature to the current list */ if (!featuremap[y*ncols+x] && val >= min_eigenvalue) { featurelist->feature[indx]->x = (KLT_locType) x; featurelist->feature[indx]->y = (KLT_locType) y; featurelist->feature[indx]->val = (int) val; /*featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_gradx = NULL; featurelist->feature[indx]->aff_grady = NULL;*/ featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; indx++; /* Fill in surrounding region of feature map, but make sure that pixels are in-bounds */ _fillFeaturemap2(x, y, featuremap, mindist, ncols, nrows); } } /* Free feature map */ //free(featuremap); } void _KLTSelectGoodFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList featurelist, selectionMode2 mode) { int npoints = 0; bool overwriteAllFeatures = (mode == SELECTING_ALL2) ? true : false; bool floatimages_created = false; int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; } if (tc->window_width < 3) { tc->window_width = 3; } if (tc->window_height < 3) { tc->window_height = 3; } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); if (!overwriteAllFeatures){ for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap2(x, y,featuremap,tc->mindist, ncols, nrows); } } } #ifdef USE_DILATE cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F); #endif /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; /* For most of the pixels in the image, do ... */ ptr = pointlist; int nSkippedPixels=tc->nSkippedPixels; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; #ifdef USE_DILATE float* eigPtr=(float*)eig.ptr(y); #endif for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]==1) { continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float _val= _minEigenvalue2(gxx, gxy, gyy); int val; if (_val > limit) { val = limit; }else{ val=(int)(_val+0.5); } if (val>tc->min_eigenvalue){ #ifdef USE_DILATE eigPtr[x] = val; #else *ptr++ = x; *ptr++ = y; *ptr++ = val; npoints++; #endif } } } } //printf("%d noints\n",npoints); #ifdef USE_DILATE cv::Mat tmp; cv::dilate(eig,tmp,cv::Mat()); cv::Size imgsize=tmp.size(); std::vector tmpCorners; for( int y = 1; y < imgsize.height - 1; y++ ) { const float* eig_data = (const float*)eig.ptr(y); const float* tmp_data = (const float*)tmp.ptr(y); for( int x = 1; x < imgsize.width - 1; x++ ) { float val = eig_data[x]; if( val != 0&& val == tmp_data[x]) tmpCorners.push_back(eig_data + x); } } std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr()); npoints=tmpCorners.size(); for (int i=0;imindist, tc->min_eigenvalue, overwriteAllFeatures); /* Free memory */ free(featuremap); free(pointlist); } void _KLTSelectGoodFeatures2(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList featurelist, selectionMode mode) { int npoints = 0; bool overwriteAllFeatures = (mode == SELECTING_ALL) ? true : false; bool floatimages_created = false; int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); if (!overwriteAllFeatures){ for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y,featuremap,tc->mindist, ncols, nrows); } } } #ifdef USE_DILATE cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F); #endif int nSkippedPixels=tc->nSkippedPixels; /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; /* For most of the pixels in the image, do ... */ ptr = pointlist; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; #ifdef USE_DILATE float* eigPtr=(float*)eig.ptr(y); #endif for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]) { continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float val= _minEigenvalue(gxx, gxy, gyy); if (val > limit) { val = (float) limit; } if (val>tc->min_eigenvalue){ #ifdef USE_DILATE eigPtr[x] = val; #else *ptr++ = x; *ptr++ = y; *ptr++ = (int) val; npoints++; #endif } } } } #ifdef USE_DILATE cv::Mat tmp; cv::dilate(eig,tmp,cv::Mat()); cv::Size imgsize=tmp.size(); std::vector tmpCorners; for( int y = 1; y < imgsize.height - 1; y++ ) { const float* eig_data = (const float*)eig.ptr(y); const float* tmp_data = (const float*)tmp.ptr(y); for( int x = 1; x < imgsize.width - 1; x++ ) { float val = eig_data[x]; if( val != 0&& val == tmp_data[x]) tmpCorners.push_back(eig_data + x); } } std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr()); npoints=tmpCorners.size(); for (int i=0;imindist, tc->min_eigenvalue, overwriteAllFeatures); /* Free memory */ free(featuremap); free(pointlist); } void KLTSelectGoodFeatures2(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; _KLTSelectGoodFeatures2(tc,sumDxx,sumDyy,sumDxy,fl,SELECTING_ALL); } void KLTReplaceLostFeatures2(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME2); } struct greaterThanPtrInt: public std::binary_function { bool operator () (const int * a, const int * b) const { return *a > *b; } }; struct smallerDistance: public std::binary_function { bool operator () (const int a, const int b) const{ float distanceY=a/sizeByGrid.width-float(sizeByGrid.height)/2.0; float distanceX=a%sizeByGrid.width-float(sizeByGrid.width)/2.0; float distanceA=distanceX*distanceX+distanceY*distanceY; distanceY=b/sizeByGrid.width-float(sizeByGrid.height)/2.0; distanceX=b%sizeByGrid.width-float(sizeByGrid.width)/2.0; float distanceB=distanceX*distanceX+distanceY*distanceY; return distanceA gridSequence; void initializeGridSequence(){ gridSequence.resize(sizeByGrid.area()); for(int i=0;iwindow_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); //grid and eigmap std::vector > grids; grids.resize(sizeByGrid.width*sizeByGrid.height); for(int i=0;inFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x,y,featuremap,tc->mindist, ncols, nrows); //register to grid int* eigPtr=(int*)eig.ptr(y); eigPtr[x]=(int)INT_MAX; int yByGrid=y/gridSize.height; int xByGrid=x/gridSize.width; grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]); } } } int nSkippedPixels=tc->nSkippedPixels; /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; int* eigPtr=(int*)eig.ptr(y); for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]) { //assert(0); continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float val= _minEigenvalue(gxx, gxy, gyy); if (val > limit) { assert(0); val = (float) limit; } if (val>tc->min_eigenvalue){ eigPtr[x]=(int)val; int yByGrid=y/gridSize.height; int xByGrid=x/gridSize.width; grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]); } } } } for(int i=0;imaxFeatureInGrid&&val!=INT_MAX){ break; }*/ if(val!=INT_MAX){ int ofs = (int)((const uchar*)grids[i][j] - eig.ptr()); int y = (int)(ofs / eig.step); int x = (int)((ofs-y*eig.step)/sizeof(int)); while (!overwriteAllFeatures &&indx < featurelist->nFeatures&& featurelist->feature[indx]->val >= 0){ indx++; } if (indx >= featurelist->nFeatures){ if(featureAdded>minFeatureInGrid){ break; }else{ featurelist->nFeatures++; featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = -1; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; } } if (!featuremap[y*ncols+x]) { featurelist->feature[indx]->x = (KLT_locType) x; featurelist->feature[indx]->y = (KLT_locType) y; featurelist->feature[indx]->val = (int) val; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; indx++; _fillFeaturemap(x, y, featuremap,tc->mindist, ncols, nrows); featureAdded++; } } } } while (indx < featurelist->nFeatures) { if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) { featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = KLT_NOT_FOUND; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; } indx++; } free(featuremap); } void KLTSelectGoodFeaturesUniform( KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; sizeByGrid.width=ncols/gridSize.width; sizeByGrid.height=nrows/gridSize.height; minFeatureInGrid=int((fl->nFeatures/float(sizeByGrid.area()))+0.5); initializeGridSequence(); _KLTSelectGoodFeaturesUniform(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL); } void KLTReplaceLostFeaturesUniform( KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME); } ================================================ FILE: GSLAM/selectGoodFeatures2.hpp ================================================ // // selectGoodFeatures.hpp // STARCK // // Created by Chaos on 3/30/16. // Copyright © 2016 Chaos. All rights reserved. // /********************************************************************* * selectGoodFeatures.c * *********************************************************************/ /* Standard includes */ #include #include /* malloc(), qsort() */ #include /* fflush() */ #include /* memset() */ #include "KLT.h" #pragma once #define KLT_NOT_FOUND -1 cv::Size gridSize(20,20); cv::Size sizeByGrid; int minFeatureInGrid; typedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode; static void _fillFeaturemap(int x, int y, uchar *featuremap, int mindist, int ncols, int nrows){ int ix, iy; for (iy = y - mindist ; iy <= y + mindist ; iy++){ for (ix = x - mindist ; ix <= x + mindist ; ix++) if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows) featuremap[iy*ncols+ix] = 1; } } /********************************************************************* * _enforceMinimumDistance * * Removes features that are within close proximity to better features. * * INPUTS * featurelist: A list of features. The nFeatures property * is used. * * OUTPUTS * featurelist: Is overwritten. Nearby "redundant" features are removed. * Writes -1's into the remaining elements. * * RETURNS * The number of remaining features. */ static int _comparePoints(const void *a, const void *b) { int v1 = *(((int *) a) + 2); int v2 = *(((int *) b) + 2); if (v1 > v2) return(-1); else if (v1 < v2) return(1); else return(0); } static void _sortPointList( int *pointlist, int npoints){ qsort(pointlist, npoints, 3*sizeof(int), _comparePoints); } static void _enforceMinimumDistance(uchar *featuremap, int *pointlist, /* featurepoints */ int npoints, /* number of featurepoints */ KLT_FeatureList featurelist, /* features */ int ncols, int nrows, /* size of images */ int mindist, /* min. dist b/w features */ int min_eigenvalue, /* min. eigenvalue */ bool overwriteAllFeatures) { int indx; /* Index into features */ int x, y, val; /* Location and trackability of pixel under consideration */ //int *featuremap; /* Boolean array recording proximity of features */ int *ptr; /* Cannot add features with an eigenvalue less than one */ if (min_eigenvalue < 1) min_eigenvalue = 1; /* Allocate memory for feature map and clear it */ //featuremap = (int *) malloc(ncols * nrows * sizeof(int)); //memset(featuremap, 0, ncols*nrows*sizeof(int)); /* Necessary because code below works with (mindist-1) */ mindist--; /* If we are keeping all old good features, then add them to the featuremap */ /*if (!overwriteAllFeatures) for (indx = 0 ; indx < featurelist->nFeatures ; indx++) if (featurelist->feature[indx]->val >= 0) { x = (int) featurelist->feature[indx]->x; y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y,INT_MAX,featuremap, mindist, ncols, nrows); } */ /* For each feature point, in descending order of importance, do ... */ ptr = pointlist; indx = 0; while (1) { /* If we can't add all the points, then fill in the rest of the featurelist with -1's */ if (ptr >= pointlist + 3*npoints) { while (indx < featurelist->nFeatures) { if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) { featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = KLT_NOT_FOUND; /* featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0;*/ } indx++; } break; } x = *ptr++; y = *ptr++; val = *ptr++; /* Ensure that feature is in-bounds */ assert(x >= 0); assert(x < ncols); assert(y >= 0); assert(y < nrows); while (!overwriteAllFeatures && indx < featurelist->nFeatures && featurelist->feature[indx]->val >= 0) indx++; if (indx >= featurelist->nFeatures) break; /* If no neighbor has been selected, and if the minimum eigenvalue is large enough, then add feature to the current list */ if (!(featuremap[y*ncols+x]) && val >= min_eigenvalue) { featurelist->feature[indx]->x = (float) x; featurelist->feature[indx]->y = (float) y; featurelist->feature[indx]->val = (int) val; /* featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0;*/ indx++; /* Fill in surrounding region of feature map, but make sure that pixels are in-bounds */ _fillFeaturemap(x, y,featuremap, mindist, ncols, nrows); } } /* Free feature map */ //free(featuremap); } /********************************************************************* * _sortPointList */ /********************************************************************* * _minEigenvalue * * Given the three distinct elements of the symmetric 2x2 matrix * [gxx gxy] * [gxy gyy], * Returns the minimum eigenvalue of the matrix. */ static float _minEigenvalue(float gxx, float gxy, float gyy) { return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f); } struct greaterThanPtr : public std::binary_function { bool operator () (const float * a, const float * b) const { return *a > *b; } }; /*********************************************************************/ void _KLTSelectGoodFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList featurelist, selectionMode mode) { int npoints = 0; bool overwriteAllFeatures = (mode == SELECTING_ALL) ? true : false; bool floatimages_created = false; int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); if (!overwriteAllFeatures){ for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y,featuremap,tc->mindist, ncols, nrows); } } } #ifdef USE_DILATE cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F); #endif int nSkippedPixels=tc->nSkippedPixels; /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; /* For most of the pixels in the image, do ... */ ptr = pointlist; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; #ifdef USE_DILATE float* eigPtr=(float*)eig.ptr(y); #endif for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]) { continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float val= _minEigenvalue(gxx, gxy, gyy); if (val > limit) { val = (float) limit; } if (val>tc->min_eigenvalue){ #ifdef USE_DILATE eigPtr[x] = val; #else *ptr++ = x; *ptr++ = y; *ptr++ = (int) val; npoints++; #endif } } } } #ifdef USE_DILATE cv::Mat tmp; cv::dilate(eig,tmp,cv::Mat()); cv::Size imgsize=tmp.size(); std::vector tmpCorners; for( int y = 1; y < imgsize.height - 1; y++ ) { const float* eig_data = (const float*)eig.ptr(y); const float* tmp_data = (const float*)tmp.ptr(y); for( int x = 1; x < imgsize.width - 1; x++ ) { float val = eig_data[x]; if( val != 0&& val == tmp_data[x]) tmpCorners.push_back(eig_data + x); } } std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr()); npoints=tmpCorners.size(); for (int i=0;imindist, tc->min_eigenvalue, overwriteAllFeatures); /* Free memory */ free(featuremap); free(pointlist); } void KLTSelectGoodFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL); } void KLTReplaceLostFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME); } typedef enum {SELECTING_ALL2, REPLACING_SOME2} selectionMode2; static void _fillFeaturemap2(int x, int y, uchar *featuremap, int mindist, int ncols, int nrows){ int ix, iy; for (iy = y - mindist ; iy <= y + mindist ; iy++){ for (ix = x - mindist ; ix <= x + mindist ; ix++) if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows) featuremap[iy*ncols+ix] = 1; } } /********************************************************************* * _enforceMinimumDistance * * Removes features that are within close proximity to better features. * * INPUTS * featurelist: A list of features. The nFeatures property * is used. * * OUTPUTS * featurelist: Is overwritten. Nearby "redundant" features are removed. * Writes -1's into the remaining elements. * * RETURNS * The number of remaining features. */ static int _comparePoints2(const void *a, const void *b) { int v1 = *(((int *) a) + 2); int v2 = *(((int *) b) + 2); if (v1 > v2) return(-1); else if (v1 < v2) return(1); else return(0); } static void _sortPointList2( int *pointlist, int npoints){ qsort(pointlist, npoints, 3*sizeof(int), _comparePoints2); } static float _minEigenvalue2(float gxx, float gxy, float gyy) { return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f); } void _enforceMinimumDistance2(uchar* featuremap, int *pointlist, /* featurepoints */ int npoints, /* number of featurepoints */ KLT_FeatureList featurelist, /* features */ int ncols, int nrows, /* size of images */ int mindist, /* min. dist b/w features */ int min_eigenvalue, /* min. eigenvalue */ bool overwriteAllFeatures) { int indx; /* Index into features */ int x, y, val; /* Location and trackability of pixel under consideration */ /* Boolean array recording proximity of features */ int *ptr; /* Cannot add features with an eigenvalue less than one */ if (min_eigenvalue < 1) min_eigenvalue = 1; //mindist--; /* Allocate memory for feature map and clear it */ /* featuremap = (uchar *) malloc(ncols * nrows * sizeof(uchar)); memset(featuremap, 0, ncols*nrows); if (!overwriteAllFeatures) for (indx = 0 ; indx < featurelist->nFeatures ; indx++) if (featurelist->feature[indx]->val >= 0) { x = (int) featurelist->feature[indx]->x; y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows); } */ /* For each feature point, in descending order of importance, do ... */ ptr = pointlist; indx = 0; while (1) { /* If we can't add all the points, then fill in the rest of the featurelist with -1's */ if (ptr >= pointlist + 3*npoints) { while (indx < featurelist->nFeatures) { if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) { featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = KLT_NOT_FOUND; /*featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_img_gradx = NULL; featurelist->feature[indx]->aff_img_grady = NULL;*/ featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; } indx++; } break; } x = *ptr++; y = *ptr++; val = *ptr++; /* Ensure that feature is in-bounds */ assert(x >= 0); assert(x < ncols); assert(y >= 0); assert(y < nrows); while (!overwriteAllFeatures && indx < featurelist->nFeatures && featurelist->feature[indx]->val >= 0) indx++; if (indx >= featurelist->nFeatures) break; /* If no neighbor has been selected, and if the minimum eigenvalue is large enough, then add feature to the current list */ if (!featuremap[y*ncols+x] && val >= min_eigenvalue) { featurelist->feature[indx]->x = (KLT_locType) x; featurelist->feature[indx]->y = (KLT_locType) y; featurelist->feature[indx]->val = (int) val; /*featurelist->feature[indx]->aff_img = NULL; featurelist->feature[indx]->aff_gradx = NULL; featurelist->feature[indx]->aff_grady = NULL;*/ featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; indx++; /* Fill in surrounding region of feature map, but make sure that pixels are in-bounds */ _fillFeaturemap2(x, y, featuremap, mindist, ncols, nrows); } } /* Free feature map */ //free(featuremap); } void _KLTSelectGoodFeatures(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList featurelist, selectionMode2 mode) { int npoints = 0; bool overwriteAllFeatures = (mode == SELECTING_ALL2) ? true : false; bool floatimages_created = false; int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1; /* Check window size (and correct if necessary) */ if (tc->window_width % 2 != 1) { tc->window_width = tc->window_width+1; } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; } if (tc->window_width < 3) { tc->window_width = 3; } if (tc->window_height < 3) { tc->window_height = 3; } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int)); uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); if (!overwriteAllFeatures){ for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap2(x, y,featuremap,tc->mindist, ncols, nrows); } } } /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; /* For most of the pixels in the image, do ... */ ptr = pointlist; int nSkippedPixels=tc->nSkippedPixels; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; #ifdef USE_DILATE float* eigPtr=(float*)eig.ptr(y); #endif for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]==1) { continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float _val= _minEigenvalue2(gxx, gxy, gyy); int val; if (_val > limit) { val = limit; }else{ val=(int)(_val+0.5); } if (val>tc->min_eigenvalue){ #ifdef USE_DILATE eigPtr[x] = val; #else *ptr++ = x; *ptr++ = y; *ptr++ = val; npoints++; #endif } } } } //printf("%d noints\n",npoints); #ifdef USE_DILATE cv::Mat tmp; cv::dilate(eig,tmp,cv::Mat()); cv::Size imgsize=tmp.size(); std::vector tmpCorners; for( int y = 1; y < imgsize.height - 1; y++ ) { const float* eig_data = (const float*)eig.ptr(y); const float* tmp_data = (const float*)tmp.ptr(y); for( int x = 1; x < imgsize.width - 1; x++ ) { float val = eig_data[x]; if( val != 0&& val == tmp_data[x]) tmpCorners.push_back(eig_data + x); } } std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr()); npoints=tmpCorners.size(); for (int i=0;imindist, tc->min_eigenvalue, overwriteAllFeatures); /* Free memory */ free(featuremap); free(pointlist); } void KLTSelectGoodFeatures2(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL2); } void KLTReplaceLostFeatures2(KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME2); } struct greaterThanPtrInt: public std::binary_function { bool operator () (const int * a, const int * b) const { return *a > *b; } }; struct smallerDistance: public std::binary_function { bool operator () (const int a, const int b) const{ float distanceY=a/sizeByGrid.width-float(sizeByGrid.height)/2.0; float distanceX=a%sizeByGrid.width-float(sizeByGrid.width)/2.0; float distanceA=distanceX*distanceX+distanceY*distanceY; distanceY=b/sizeByGrid.width-float(sizeByGrid.height)/2.0; distanceX=b%sizeByGrid.width-float(sizeByGrid.width)/2.0; float distanceB=distanceX*distanceX+distanceY*distanceY; return distanceA gridSequence; void initializeGridSequence(){ gridSequence.resize(sizeByGrid.area()); for(int i=0;iwindow_width % 2 != 1) { tc->window_width = tc->window_width+1; KLTWarning("Tracking context's window width must be odd. " "Changing to %d.\n", tc->window_width); } if (tc->window_height % 2 != 1) { tc->window_height = tc->window_height+1; KLTWarning("Tracking context's window height must be odd. " "Changing to %d.\n", tc->window_height); } if (tc->window_width < 3) { tc->window_width = 3; KLTWarning("Tracking context's window width must be at least three. \n" "Changing to %d.\n", tc->window_width); } if (tc->window_height < 3) { tc->window_height = 3; KLTWarning("Tracking context's window height must be at least three. \n" "Changing to %d.\n", tc->window_height); } int window_hw = tc->window_width/2; int window_hh = tc->window_height/2; /* Create pointlist, which is a simplified version of a featurelist, */ /* for speed. Contains only integer locations and values. */ uchar *featuremap=(uchar*)malloc(ncols*nrows); memset(featuremap,0,ncols*nrows); //grid and eigmap std::vector > grids; grids.resize(sizeByGrid.width*sizeByGrid.height); for(int i=0;inFeatures ; indx++){ if (featurelist->feature[indx]->val >= 0) { int x = (int) featurelist->feature[indx]->x; int y = (int) featurelist->feature[indx]->y; _fillFeaturemap(x,y,featuremap,tc->mindist, ncols, nrows); //register to grid int* eigPtr=(int*)eig.ptr(y); eigPtr[x]=(int)INT_MAX; int yByGrid=y/gridSize.height; int xByGrid=x/gridSize.width; grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]); } } } int nSkippedPixels=tc->nSkippedPixels; /* Compute trackability of each image pixel as the minimum of the two eigenvalues of the Z matrix */ { float gxx, gxy, gyy; int *ptr; float val; unsigned int limit = 1; int borderx = tc->borderx; /* Must not touch cols */ int bordery = tc->bordery; /* lost by convolution */ if (borderx < window_hw){ borderx = window_hw; } if (bordery < window_hh){ bordery = window_hh; } /* Find largest value of an int */ for (int i = 0 ; i < sizeof(int) ; i++){ limit *= 256; } limit = limit/2 - 1; for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){ float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)}; float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)}; float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)}; int* eigPtr=(int*)eig.ptr(y); for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1; x < ncols - borderx ; x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) { if (featuremap[y*ncols+x]) { //assert(0); continue; } /* Sum the gradients in the surrounding window */ gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2]; gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2]; gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2]; /* Store the trackability of the pixel as the minimum of the two eigenvalues */ float val= _minEigenvalue(gxx, gxy, gyy); if (val > limit) { //assert(0); val = (float) limit; } if (val>tc->min_eigenvalue){ eigPtr[x]=(int)val; int yByGrid=y/gridSize.height; int xByGrid=x/gridSize.width; grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]); } } } } for(int i=0;imaxFeatureInGrid&&val!=INT_MAX){ break; }*/ if(val!=INT_MAX){ int ofs = (int)((const uchar*)grids[i][j] - eig.ptr()); int y = (int)(ofs / eig.step); int x = (int)((ofs-y*eig.step)/sizeof(int)); while (!overwriteAllFeatures &&indx < featurelist->nFeatures&& featurelist->feature[indx]->val >= 0){ indx++; } if (indx >= featurelist->nFeatures){ if(featureAdded>minFeatureInGrid){ break; }else{ featurelist->nFeatures++; featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = -1; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; } } if (!featuremap[y*ncols+x]) { featurelist->feature[indx]->x = (KLT_locType) x; featurelist->feature[indx]->y = (KLT_locType) y; featurelist->feature[indx]->val = (int) val; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; indx++; _fillFeaturemap(x, y, featuremap,tc->mindist, ncols, nrows); featureAdded++; } } } } while (indx < featurelist->nFeatures) { if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) { featurelist->feature[indx]->x = -1; featurelist->feature[indx]->y = -1; featurelist->feature[indx]->val = KLT_NOT_FOUND; featurelist->feature[indx]->aff_x = -1.0; featurelist->feature[indx]->aff_y = -1.0; featurelist->feature[indx]->aff_Axx = 1.0; featurelist->feature[indx]->aff_Ayx = 0.0; featurelist->feature[indx]->aff_Axy = 0.0; featurelist->feature[indx]->aff_Ayy = 1.0; } indx++; } free(featuremap); } void KLTSelectGoodFeaturesUniform( KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; sizeByGrid.width=ncols/gridSize.width; sizeByGrid.height=nrows/gridSize.height; minFeatureInGrid=int((fl->nFeatures/float(sizeByGrid.area()))+0.5); initializeGridSequence(); _KLTSelectGoodFeaturesUniform(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL); } void KLTReplaceLostFeaturesUniform( KLT_TrackingContext tc, const cv::Mat& sumDxx, const cv::Mat& sumDyy, const cv::Mat& sumDxy, KLT_FeatureList fl){ int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); int ncols=sumDxx.cols-1; int nrows=sumDxx.rows-1; if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME); } ================================================ FILE: GSLAM/svdlib.cpp ================================================ /* Copyright © 2002, University of Tennessee Research Foundation. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the University of Tennessee nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "svdlib.hpp" #include "svdutil.hpp" char *SVDVersion = "1.4"; long SVDVerbosity = 0; long SVDCount[SVD_COUNTERS]; void svdResetCounters(void) { int i; for (i = 0; i < SVD_COUNTERS; i++) SVDCount[i] = 0; } /********************************* Allocation ********************************/ /* Row major order. Rows are vectors that are consecutive in memory. Matrix is initialized to empty. */ DMat svdNewDMat(int rows, int cols) { int i; DMat D = (DMat) malloc(sizeof(struct dmat)); if (!D) {perror("svdNewDMat"); return NULL;} D->rows = rows; D->cols = cols; D->value = (double **) malloc(rows * sizeof(double *)); if (!D->value) {SAFE_FREE(D); return NULL;} D->value[0] = (double *) calloc(rows * cols, sizeof(double)); if (!D->value[0]) {SAFE_FREE(D->value); SAFE_FREE(D); return NULL;} for (i = 1; i < rows; i++) D->value[i] = D->value[i-1] + cols; return D; } void svdFreeDMat(DMat D) { if (!D) return; SAFE_FREE(D->value[0]); SAFE_FREE(D->value); free(D); } SMat svdNewSMat(int rows, int cols, int vals) { SMat S = (SMat) calloc(1, sizeof(struct smat)); if (!S) {perror("svdNewSMat"); return NULL;} S->rows = rows; S->cols = cols; S->vals = vals; S->pointr = svd_longArray(cols + 1, TRUE, "svdNewSMat: pointr"); if (!S->pointr) {svdFreeSMat(S); return NULL;} S->rowind = svd_longArray(vals, FALSE, "svdNewSMat: rowind"); if (!S->rowind) {svdFreeSMat(S); return NULL;} S->value = svd_doubleArray(vals, FALSE, "svdNewSMat: value"); if (!S->value) {svdFreeSMat(S); return NULL;} return S; } void svdFreeSMat(SMat S) { if (!S) return; SAFE_FREE(S->pointr); SAFE_FREE(S->rowind); SAFE_FREE(S->value); free(S); } /* Creates an empty SVD record */ SVDRec svdNewSVDRec(void) { SVDRec R = (SVDRec) calloc(1, sizeof(struct svdrec)); if (!R) {perror("svdNewSVDRec"); return NULL;} return R; } /* Frees an svd rec and all its contents. */ void svdFreeSVDRec(SVDRec R) { if (!R) return; if (R->Ut) svdFreeDMat(R->Ut); if (R->S) SAFE_FREE(R->S); if (R->Vt) svdFreeDMat(R->Vt); free(R); } /**************************** Conversion *************************************/ /* Converts a sparse matrix to a dense one (without affecting the former) */ DMat svdConvertStoD(SMat S) { int i, c; DMat D = svdNewDMat(S->rows, S->cols); if (!D) { svd_error("svdConvertStoD: failed to allocate D"); return NULL; } for (i = 0, c = 0; i < S->vals; i++) { while (S->pointr[c + 1] <= i) c++; D->value[S->rowind[i]][c] = S->value[i]; } return D; } /* Converts a dense matrix to a sparse one (without affecting the dense one) */ SMat svdConvertDtoS(DMat D) { SMat S; int i, j, n; for (i = 0, n = 0; i < D->rows; i++) for (j = 0; j < D->cols; j++) if (D->value[i][j] != 0) n++; S = svdNewSMat(D->rows, D->cols, n); if (!S) { svd_error("svdConvertDtoS: failed to allocate S"); return NULL; } for (j = 0, n = 0; j < D->cols; j++) { S->pointr[j] = n; for (i = 0; i < D->rows; i++) if (D->value[i][j] != 0) { S->rowind[n] = i; S->value[n] = D->value[i][j]; n++; } } S->pointr[S->cols] = S->vals; return S; } /* Transposes a dense matrix. */ DMat svdTransposeD(DMat D) { int r, c; DMat N = svdNewDMat(D->cols, D->rows); for (r = 0; r < D->rows; r++) for (c = 0; c < D->cols; c++) N->value[c][r] = D->value[r][c]; return N; } /* Efficiently transposes a sparse matrix. */ SMat svdTransposeS(SMat S) { int r, c, i, j; SMat N = svdNewSMat(S->cols, S->rows, S->vals); /* Count number nz in each row. */ for (i = 0; i < S->vals; i++) N->pointr[S->rowind[i]]++; /* Fill each cell with the starting point of the previous row. */ N->pointr[S->rows] = S->vals - N->pointr[S->rows - 1]; for (r = S->rows - 1; r > 0; r--) N->pointr[r] = N->pointr[r+1] - N->pointr[r-1]; N->pointr[0] = 0; /* Assign the new columns and values. */ for (c = 0, i = 0; c < S->cols; c++) { for (; i < S->pointr[c+1]; i++) { r = S->rowind[i]; j = N->pointr[r+1]++; N->rowind[j] = c; N->value[j] = S->value[i]; } } return N; } /**************************** Input/Output ***********************************/ void svdWriteDenseArray(double *a, int n, char *filename, char binary) { int i; FILE *file = svd_writeFile(filename, FALSE); if (!file) return svd_error("svdWriteDenseArray: failed to write %s", filename); if (binary) { svd_writeBinInt(file, n); for (i = 0; i < n; i++) svd_writeBinFloat(file, (float) a[i]); } else { fprintf(file, "%d\n", n); for (i = 0; i < n; i++) fprintf(file, "%g\n", a[i]); } svd_closeFile(file); } double *svdLoadDenseArray(char *filename, int *np, char binary) { int i, n; double *a; FILE *file = svd_readFile(filename); if (!file) { svd_error("svdLoadDenseArray: failed to read %s", filename); return NULL; } if (binary) { svd_readBinInt(file, np); } else if (fscanf(file, " %d", np) != 1) { svd_error("svdLoadDenseArray: error reading %s", filename); svd_closeFile(file); return NULL; } n = *np; a = svd_doubleArray(n, FALSE, "svdLoadDenseArray: a"); if (!a) return NULL; if (binary) { float f; for (i = 0; i < n; i++) { svd_readBinFloat(file, &f); a[i] = f; } } else { for (i = 0; i < n; i++) { if (fscanf(file, " %lf\n", a + i) != 1) { svd_error("svdLoadDenseArray: error reading %s", filename); break; } } } svd_closeFile(file); return a; } /* File format has a funny header, then first entry index per column, then the row for each entry, then the value for each entry. Indices count from 1. Assumes A is initialized. */ static SMat svdLoadSparseTextHBFile(FILE *file) { char line[128]; long i, x, rows, cols, vals, num_mat; SMat S; /* Skip the header line: */ if (!fgets(line, 128, file)); /* Skip the line giving the number of lines in this file: */ if (!fgets(line, 128, file)); /* Read the line with useful dimensions: */ if (fscanf(file, "%*s%ld%ld%ld%ld\n", &rows, &cols, &vals, &num_mat) != 4) { svd_error("svdLoadSparseTextHBFile: bad file format on line 3"); return NULL; } if (num_mat != 0) { svd_error("svdLoadSparseTextHBFile: I don't know how to handle a file " "with elemental matrices (last entry on header line 3)"); return NULL; } /* Skip the line giving the formats: */ if (!fgets(line, 128, file)); S = svdNewSMat(rows, cols, vals); if (!S) return NULL; /* Read column pointers. */ for (i = 0; i <= S->cols; i++) { if (fscanf(file, " %ld", &x) != 1) { svd_error("svdLoadSparseTextHBFile: error reading pointr %d", i); return NULL; } S->pointr[i] = x - 1; } S->pointr[S->cols] = S->vals; /* Read row indices. */ for (i = 0; i < S->vals; i++) { if (fscanf(file, " %ld", &x) != 1) { svd_error("svdLoadSparseTextHBFile: error reading rowind %d", i); return NULL; } S->rowind[i] = x - 1; } for (i = 0; i < S->vals; i++) if (fscanf(file, " %lf", S->value + i) != 1) { svd_error("svdLoadSparseTextHBFile: error reading value %d", i); return NULL; } return S; } static void svdWriteSparseTextHBFile(SMat S, FILE *file) { int i; long col_lines = ((S->cols + 1) / 8) + (((S->cols + 1) % 8) ? 1 : 0); long row_lines = (S->vals / 8) + ((S->vals % 8) ? 1 : 0); long total_lines = col_lines + 2 * row_lines; char title[32]; sprintf(title, "SVDLIBC v. %s", SVDVersion); fprintf(file, "%-72s%-8s\n", title, ""); fprintf(file, "%14ld%14ld%14ld%14ld%14d\n", total_lines, col_lines, row_lines, row_lines, 0); fprintf(file, "%-14s%14ld%14ld%14ld%14d\n", "rra", S->rows, S->cols, S->vals, 0); fprintf(file, "%16s%16s%16s%16s\n", "(8i)", "(8i)", "(8e)", "(8e)"); for (i = 0; i <= S->cols; i++) fprintf(file, "%ld%s", S->pointr[i] + 1, (((i+1) % 8) == 0) ? "\n" : " "); fprintf(file, "\n"); for (i = 0; i < S->vals; i++) fprintf(file, "%ld%s", S->rowind[i] + 1, (((i+1) % 8) == 0) ? "\n" : " "); fprintf(file, "\n"); for (i = 0; i < S->vals; i++) fprintf(file, "%g%s", S->value[i], (((i+1) % 8) == 0) ? "\n" : " "); fprintf(file, "\n"); } static SMat svdLoadSparseTextFile(FILE *file) { long c, i, n, v, rows, cols, vals; SMat S; if (fscanf(file, " %ld %ld %ld", &rows, &cols, &vals) != 3) { svd_error("svdLoadSparseTextFile: bad file format"); return NULL; } S = svdNewSMat(rows, cols, vals); if (!S) return NULL; for (c = 0, v = 0; c < cols; c++) { if (fscanf(file, " %ld", &n) != 1) { svd_error("svdLoadSparseTextFile: bad file format"); return NULL; } S->pointr[c] = v; for (i = 0; i < n; i++, v++) { if (fscanf(file, " %ld %lf", S->rowind + v, S->value + v) != 2) { svd_error("svdLoadSparseTextFile: bad file format"); return NULL; } } } S->pointr[cols] = vals; return S; } static void svdWriteSparseTextFile(SMat S, FILE *file) { int c, v; fprintf(file, "%ld %ld %ld\n", S->rows, S->cols, S->vals); for (c = 0, v = 0; c < S->cols; c++) { fprintf(file, "%ld\n", S->pointr[c + 1] - S->pointr[c]); for (; v < S->pointr[c+1]; v++) fprintf(file, "%ld %g\n", S->rowind[v], S->value[v]); } } static SMat svdLoadSparseBinaryFile(FILE *file) { int rows, cols, vals, n, c, i, v, r, e = 0; float f; SMat S; e += svd_readBinInt(file, &rows); e += svd_readBinInt(file, &cols); e += svd_readBinInt(file, &vals); if (e) { svd_error("svdLoadSparseBinaryFile: bad file format"); return NULL; } S = svdNewSMat(rows, cols, vals); if (!S) return NULL; for (c = 0, v = 0; c < cols; c++) { if (svd_readBinInt(file, &n)) { svd_error("svdLoadSparseBinaryFile: bad file format"); return NULL; } S->pointr[c] = v; for (i = 0; i < n; i++, v++) { e += svd_readBinInt(file, &r); e += svd_readBinFloat(file, &f); if (e) { svd_error("svdLoadSparseBinaryFile: bad file format"); return NULL; } S->rowind[v] = r; S->value[v] = f; } } S->pointr[cols] = vals; return S; } static void svdWriteSparseBinaryFile(SMat S, FILE *file) { int c, v; svd_writeBinInt(file, (int) S->rows); svd_writeBinInt(file, (int) S->cols); svd_writeBinInt(file, (int) S->vals); for (c = 0, v = 0; c < S->cols; c++) { svd_writeBinInt(file, (int) (S->pointr[c + 1] - S->pointr[c])); for (; v < S->pointr[c+1]; v++) { svd_writeBinInt(file, (int) S->rowind[v]); svd_writeBinFloat(file, (float) S->value[v]); } } } static DMat svdLoadDenseTextFile(FILE *file) { long rows, cols, i, j; DMat D; if (fscanf(file, " %ld %ld", &rows, &cols) != 2) { svd_error("svdLoadDenseTextFile: bad file format"); return NULL; } D = svdNewDMat(rows, cols); if (!D) return NULL; for (i = 0; i < rows; i++) for (j = 0; j < cols; j++) { if (fscanf(file, " %lf", &(D->value[i][j])) != 1) { svd_error("svdLoadDenseTextFile: bad file format"); return NULL; } } return D; } static void svdWriteDenseTextFile(DMat D, FILE *file) { int i, j; fprintf(file, "%ld %ld\n", D->rows, D->cols); for (i = 0; i < D->rows; i++) for (j = 0; j < D->cols; j++) fprintf(file, "%g%c", D->value[i][j], (j == D->cols - 1) ? '\n' : ' '); } static DMat svdLoadDenseBinaryFile(FILE *file) { int rows, cols, i, j, e = 0; float f; DMat D; e += svd_readBinInt(file, &rows); e += svd_readBinInt(file, &cols); if (e) { svd_error("svdLoadDenseBinaryFile: bad file format"); return NULL; } D = svdNewDMat(rows, cols); if (!D) return NULL; for (i = 0; i < rows; i++) for (j = 0; j < cols; j++) { if (svd_readBinFloat(file, &f)) { svd_error("svdLoadDenseBinaryFile: bad file format"); return NULL; } D->value[i][j] = f; } return D; } static void svdWriteDenseBinaryFile(DMat D, FILE *file) { int i, j; svd_writeBinInt(file, (int) D->rows); svd_writeBinInt(file, (int) D->cols); for (i = 0; i < D->rows; i++) for (j = 0; j < D->cols; j++) svd_writeBinFloat(file, (float) D->value[i][j]); } SMat svdLoadSparseMatrix(char *filename, int format) { SMat S = NULL; DMat D = NULL; FILE *file = svd_fatalReadFile(filename); switch (format) { case SVD_F_STH: S = svdLoadSparseTextHBFile(file); break; case SVD_F_ST: S = svdLoadSparseTextFile(file); break; case SVD_F_SB: S = svdLoadSparseBinaryFile(file); break; case SVD_F_DT: D = svdLoadDenseTextFile(file); break; case SVD_F_DB: D = svdLoadDenseBinaryFile(file); break; default: svd_error("svdLoadSparseMatrix: unknown format %d", format); } svd_closeFile(file); if (D) { S = svdConvertDtoS(D); svdFreeDMat(D); } return S; } DMat svdLoadDenseMatrix(char *filename, int format) { SMat S = NULL; DMat D = NULL; FILE *file = svd_fatalReadFile(filename); switch (format) { case SVD_F_STH: S = svdLoadSparseTextHBFile(file); break; case SVD_F_ST: S = svdLoadSparseTextFile(file); break; case SVD_F_SB: S = svdLoadSparseBinaryFile(file); break; case SVD_F_DT: D = svdLoadDenseTextFile(file); break; case SVD_F_DB: D = svdLoadDenseBinaryFile(file); break; default: svd_error("svdLoadSparseMatrix: unknown format %d", format); } svd_closeFile(file); if (S) { D = svdConvertStoD(S); svdFreeSMat(S); } return D; } void svdWriteSparseMatrix(SMat S, char *filename, int format) { DMat D = NULL; FILE *file = svd_writeFile(filename, FALSE); if (!file) { svd_error("svdWriteSparseMatrix: failed to write file %s\n", filename); return; } switch (format) { case SVD_F_STH: svdWriteSparseTextHBFile(S, file); break; case SVD_F_ST: svdWriteSparseTextFile(S, file); break; case SVD_F_SB: svdWriteSparseBinaryFile(S, file); break; case SVD_F_DT: D = svdConvertStoD(S); svdWriteDenseTextFile(D, file); break; case SVD_F_DB: D = svdConvertStoD(S); svdWriteDenseBinaryFile(D, file); break; default: svd_error("svdLoadSparseMatrix: unknown format %d", format); } svd_closeFile(file); if (D) svdFreeDMat(D); } void svdWriteDenseMatrix(DMat D, char *filename, int format) { SMat S = NULL; FILE *file = svd_writeFile(filename, FALSE); if (!file) { svd_error("svdWriteDenseMatrix: failed to write file %s\n", filename); return; } switch (format) { case SVD_F_STH: S = svdConvertDtoS(D); svdWriteSparseTextHBFile(S, file); break; case SVD_F_ST: S = svdConvertDtoS(D); svdWriteSparseTextFile(S, file); break; case SVD_F_SB: S = svdConvertDtoS(D); svdWriteSparseBinaryFile(S, file); break; case SVD_F_DT: svdWriteDenseTextFile(D, file); break; case SVD_F_DB: svdWriteDenseBinaryFile(D, file); break; default: svd_error("svdLoadSparseMatrix: unknown format %d", format); } svd_closeFile(file); if (S) svdFreeSMat(S); } ================================================ FILE: GSLAM/svdlib.hpp ================================================ /* Copyright © 2002, University of Tennessee Research Foundation. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the University of Tennessee nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #ifndef SVDLIB_H #define SVDLIB_H #ifndef FALSE # define FALSE 0 #endif #ifndef TRUE # define TRUE 1 #endif /******************************** Structures *********************************/ typedef struct smat *SMat; typedef struct dmat *DMat; typedef struct svdrec *SVDRec; static SVDRec globalSVDRec; const bool useInitialization=true; /* Harwell-Boeing sparse matrix. */ struct smat { long rows; long cols; long vals; /* Total non-zero entries. */ long *pointr; /* For each col (plus 1), index of first non-zero entry. */ long *rowind; /* For each nz entry, the row index. */ double *value; /* For each nz entry, the value. */ }; /* Row-major dense matrix. Rows are consecutive vectors. */ struct dmat { long rows; long cols; double **value; /* Accessed by [row][col]. Free value[0] and value to free.*/ }; struct svdrec { int d; /* Dimensionality (rank) */ DMat Ut; /* Transpose of left singular vectors. (d by m) The vectors are the rows of Ut. */ double *S; /* Array of singular values. (length d) */ DMat Vt; /* Transpose of right singular vectors. (d by n) The vectors are the rows of Vt. */ }; /******************************** Variables **********************************/ /* Version info */ extern char *SVDVersion; /* How verbose is the package: 0, 1 (default), 2 */ extern long SVDVerbosity; /* Counter(s) used to track how much work is done in computing the SVD. */ enum svdCounters {SVD_MXV, SVD_COUNTERS}; extern long SVDCount[SVD_COUNTERS]; extern void svdResetCounters(void); enum svdFileFormats {SVD_F_STH, SVD_F_ST, SVD_F_SB, SVD_F_DT, SVD_F_DB}; /* File formats: SVD_F_STH: sparse text, SVDPACK-style SVD_F_ST: sparse text, SVDLIB-style SVD_F_DT: dense text SVD_F_SB: sparse binary SVD_F_DB: dense binary */ /* True if a file format is sparse: */ #define SVD_IS_SPARSE(format) ((format >= SVD_F_STH) && (format <= SVD_F_SB)) /******************************** Functions **********************************/ /* Creates an empty dense matrix. */ extern DMat svdNewDMat(int rows, int cols); /* Frees a dense matrix. */ extern void svdFreeDMat(DMat D); /* Creates an empty sparse matrix. */ SMat svdNewSMat(int rows, int cols, int vals); /* Frees a sparse matrix. */ void svdFreeSMat(SMat S); /* Creates an empty SVD record. */ SVDRec svdNewSVDRec(void); /* Frees an svd rec and all its contents. */ void svdFreeSVDRec(SVDRec R); /* Converts a sparse matrix to a dense one (without affecting former) */ DMat svdConvertStoD(SMat S); /* Converts a dense matrix to a sparse one (without affecting former) */ SMat svdConvertDtoS(DMat D); /* Transposes a dense matrix (returning a new one) */ DMat svdTransposeD(DMat D); /* Transposes a sparse matrix (returning a new one) */ SMat svdTransposeS(SMat S); /* Writes an array to a file. */ extern void svdWriteDenseArray(double *a, int n, char *filename, char binary); /* Reads an array from a file, storing its size in *np. */ extern double *svdLoadDenseArray(char *filename, int *np, char binary); /* Loads a matrix file (in various formats) into a sparse matrix. */ extern SMat svdLoadSparseMatrix(char *filename, int format); /* Loads a matrix file (in various formats) into a dense matrix. */ extern DMat svdLoadDenseMatrix(char *filename, int format); /* Writes a dense matrix to a file in a given format. */ extern void svdWriteDenseMatrix(DMat A, char *filename, int format); /* Writes a sparse matrix to a file in a given format. */ extern void svdWriteSparseMatrix(SMat A, char *filename, int format); /* Performs the las2 SVD algorithm and returns the resulting Ut, S, and Vt. */ extern SVDRec svdLAS2(SMat A, long dimensions, long iterations, double end[2], double kappa); /* Chooses default parameter values. Set dimensions to 0 for all dimensions: */ extern SVDRec svdLAS2A(SMat A, long dimensions); #endif /* SVDLIB_H */ ================================================ FILE: GSLAM/svdutil.cpp ================================================ /* Copyright © 2002, University of Tennessee Research Foundation. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the University of Tennessee nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include #include #include #include #include "svdlib.hpp" #include "svdutil.hpp" #define BUNZIP2 "bzip2 -d" #define BZIP2 "bzip2 -1" #define UNZIP "gzip -d" #define ZIP "gzip -1" #define COMPRESS "compress" #define MAX_FILENAME 512 #define MAX_PIPES 64 static FILE *Pipe[MAX_PIPES]; static int numPipes = 0; long *svd_longArray(long size, char empty, char *name) { long *a; if (empty) a = (long *) calloc(size, sizeof(long)); else a = (long *) malloc(size * sizeof(long)); if (!a) { perror(name); /* exit(errno); */ } return a; } double *svd_doubleArray(long size, char empty, char *name) { double *a; if (empty) a = (double *) calloc(size, sizeof(double)); else a = (double *) malloc(size * sizeof(double)); if (!a) { perror(name); /* exit(errno); */ } return a; } void svd_beep(void) { fputc('\a', stderr); fflush(stderr); } void svd_debug(char *fmt, ...) { va_list args; va_start(args, fmt); vfprintf(stderr, fmt, args); va_end(args); } void svd_error(char *fmt, ...) { va_list args; va_start(args, fmt); svd_beep(); fprintf(stderr, "ERROR: "); vfprintf(stderr, fmt, args); fprintf(stderr, "\n"); va_end(args); } void svd_fatalError(char *fmt, ...) { va_list args; va_start(args, fmt); svd_beep(); fprintf(stderr, "ERROR: "); vfprintf(stderr, fmt, args); fprintf(stderr, "\a\n"); va_end(args); exit(1); } static void registerPipe(FILE *p) { if (numPipes >= MAX_PIPES) svd_error("Too many pipes open"); Pipe[numPipes++] = p; } static char isPipe(FILE *p) { int i; for (i = 0; i < numPipes && Pipe[i] != p; i++); if (i == numPipes) return FALSE; Pipe[i] = Pipe[--numPipes]; return TRUE; } static FILE *openPipe(char *pipeName, char *mode) { FILE *pipe; fflush(stdout); if ((pipe = popen(pipeName, mode))) registerPipe(pipe); return pipe; } static FILE *readZippedFile(char *command, char *fileName) { char buf[MAX_FILENAME]; sprintf(buf, "%s < %s 2>/dev/null", command, fileName); return openPipe(buf, "r"); } FILE *svd_fatalReadFile(char *filename) { FILE *file; if (!(file = svd_readFile(filename))) svd_fatalError("couldn't read the file %s", filename); return file; } static int stringEndsIn(char *s, char *t) { int ls = strlen(s); int lt = strlen(t); if (ls < lt) return FALSE; return (strcmp(s + ls - lt, t)) ? FALSE : TRUE; } /* Will silently return NULL if file couldn't be opened */ FILE *svd_readFile(char *fileName) { char fileBuf[MAX_FILENAME]; struct stat statbuf; /* Special file name */ if (!strcmp(fileName, "-")) return stdin; /* If it is a pipe */ if (fileName[0] == '|') return openPipe(fileName + 1, "r"); /* Check if already ends in .gz or .Z and assume compressed */ if (stringEndsIn(fileName, ".gz") || stringEndsIn(fileName, ".Z")) { if (!stat(fileName, &statbuf)) return readZippedFile(UNZIP, fileName); return NULL; } /* Check if already ends in .bz or .bz2 and assume compressed */ if (stringEndsIn(fileName, ".bz") || stringEndsIn(fileName, ".bz2")) { if (!stat(fileName, &statbuf)) return readZippedFile(BUNZIP2, fileName); return NULL; } /* Try just opening normally */ if (!stat(fileName, &statbuf)) return fopen(fileName, "r"); /* Try adding .gz */ sprintf(fileBuf, "%s.gz", fileName); if (!stat(fileBuf, &statbuf)) return readZippedFile(UNZIP, fileBuf); /* Try adding .Z */ sprintf(fileBuf, "%s.Z", fileName); if (!stat(fileBuf, &statbuf)) return readZippedFile(UNZIP, fileBuf); /* Try adding .bz2 */ sprintf(fileBuf, "%s.bz2", fileName); if (!stat(fileBuf, &statbuf)) return readZippedFile(BUNZIP2, fileBuf); /* Try adding .bz */ sprintf(fileBuf, "%s.bz", fileName); if (!stat(fileBuf, &statbuf)) return readZippedFile(BUNZIP2, fileBuf); return NULL; } static FILE *writeZippedFile(char *fileName, char append) { char buf[MAX_FILENAME]; const char *op = (append) ? ">>" : ">"; if (stringEndsIn(fileName, ".bz2") || stringEndsIn(fileName, ".bz")) sprintf(buf, "%s %s \"%s\"", BZIP2, op, fileName); else if (stringEndsIn(fileName, ".Z")) sprintf(buf, "%s %s \"%s\"", COMPRESS, op, fileName); else sprintf(buf, "%s %s \"%s\"", ZIP, op, fileName); return openPipe(buf, "w"); } FILE *svd_writeFile(char *fileName, char append) { /* Special file name */ if (!strcmp(fileName, "-")) return stdout; /* If it is a pipe */ if (fileName[0] == '|') return openPipe(fileName + 1, "w"); /* Check if ends in .gz, .Z, .bz, .bz2 */ if (stringEndsIn(fileName, ".gz") || stringEndsIn(fileName, ".Z") || stringEndsIn(fileName, ".bz") || stringEndsIn(fileName, ".bz2")) return writeZippedFile(fileName, append); return (append) ? fopen(fileName, "a") : fopen(fileName, "w"); } /* Could be a file or a stream. */ void svd_closeFile(FILE *file) { if (file == stdin || file == stdout) return; if (isPipe(file)) pclose(file); else fclose(file); } char svd_readBinInt(FILE *file, int *val) { int x; if (fread(&x, sizeof(int), 1, file) == 1) { *val = ntohl(x); return FALSE; } return TRUE; } /* This reads a float in network order and converts to a real in host order. */ char svd_readBinFloat(FILE *file, float *val) { int x; float y; if (fread(&x, sizeof(int), 1, file) == 1) { x = ntohl(x); y = *((float *) &x); *val = y; return FALSE; } return TRUE; } char svd_writeBinInt(FILE *file, int x) { int y = htonl(x); if (fwrite(&y, sizeof(int), 1, file) != 1) return TRUE; return FALSE; } /* This takes a real in host order and writes a float in network order. */ char svd_writeBinFloat(FILE *file, float r) { int y = htonl(*((int *) &r)); if (fwrite(&y, sizeof(int), 1, file) != 1) return TRUE; return FALSE; } /************************************************************** * returns |a| if b is positive; else fsign returns -|a| * **************************************************************/ double svd_fsign(double a, double b) { if ((a>=0.0 && b>=0.0) || (a<0.0 && b<0.0))return(a); else return -a; } /************************************************************** * returns the larger of two double precision numbers * **************************************************************/ double svd_dmax(double a, double b) { return (a > b) ? a : b; } /************************************************************** * returns the smaller of two double precision numbers * **************************************************************/ double svd_dmin(double a, double b) { return (a < b) ? a : b; } /************************************************************** * returns the larger of two integers * **************************************************************/ long svd_imax(long a, long b) { return (a > b) ? a : b; } /************************************************************** * returns the smaller of two integers * **************************************************************/ long svd_imin(long a, long b) { return (a < b) ? a : b; } /************************************************************** * Function scales a vector by a constant. * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ void svd_dscal(long n, double da, double *dx, long incx) { #ifndef USE_BLAS cblas_dscal(n,da,dx,incx); #else long i; if (n <= 0 || incx == 0) return; if (incx < 0) dx += (-n+1) * incx; for (i=0; i < n; i++) { *dx *= da; dx += incx; } #endif return; } /************************************************************** * function scales a vector by a constant. * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ void svd_datx(long n, double da, double *dx, long incx, double *dy, long incy) { #ifdef USE_BLAS memset(dy,0,n*sizeof(double)); cblas_daxpy(n,da,dx,incx,dy,incy); #else long i; if (n <= 0 || incx == 0 || incy == 0 || da == 0.0) return; if (incx == 1 && incy == 1) for (i=0; i < n; i++) *dy++ = da * (*dx++); else { if (incx < 0) dx += (-n+1) * incx; if (incy < 0) dy += (-n+1) * incy; for (i=0; i < n; i++) { *dy = da * (*dx); dx += incx; dy += incy; } } #endif return; } /************************************************************** * Function copies a vector x to a vector y * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ void svd_dcopy(long n, double *dx, long incx, double *dy, long incy) { #ifdef USE_BLAS cblas_dcopy(n,dx,incx,dy,incy); #else long i; if (n <= 0 || incx == 0 || incy == 0) return; if (incx == 1 && incy == 1) for (i=0; i < n; i++) *dy++ = *dx++; else { if (incx < 0) dx += (-n+1) * incx; if (incy < 0) dy += (-n+1) * incy; for (i=0; i < n; i++) { *dy = *dx; dx += incx; dy += incy; } } #endif return; } /************************************************************** * Function forms the dot product of two vectors. * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ double svd_ddot(long n, double *dx, long incx, double *dy, long incy) { double dot_product; #ifdef USE_BLAS dot_product=cblas_ddot(n,dx,incx,dy,incy); #else long i; if (n <= 0 || incx == 0 || incy == 0) return(0.0); dot_product = 0.0; if (incx == 1 && incy == 1) for (i=0; i < n; i++) dot_product += (*dx++) * (*dy++); else { if (incx < 0) dx += (-n+1) * incx; if (incy < 0) dy += (-n+1) * incy; for (i=0; i < n; i++) { dot_product += (*dx) * (*dy); dx += incx; dy += incy; } } #endif return(dot_product); } /************************************************************** * Constant times a vector plus a vector * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ void svd_daxpy (long n, double da, double *dx, long incx, double *dy, long incy) { long i; #ifdef USE_BLAS cblas_daxpy(n,da,dx,incx,dy,incy); #else if (n <= 0 || incx == 0 || incy == 0 || da == 0.0) return; if (incx == 1 && incy == 1) for (i=0; i < n; i++) { *dy += da * (*dx++); dy++; } else { if (incx < 0) dx += (-n+1) * incx; if (incy < 0) dy += (-n+1) * incy; for (i=0; i < n; i++) { *dy += da * (*dx); dx += incx; dy += incy; } } #endif return; } /********************************************************************* * Function sorts array1 and array2 into increasing order for array1 * *********************************************************************/ void svd_dsort2(long igap, long n, double *array1, double *array2) { double temp; long i, j, index; if (!igap) return; else { for (i = igap; i < n; i++) { j = i - igap; index = i; while (j >= 0 && array1[j] > array1[index]) { temp = array1[j]; array1[j] = array1[index]; array1[index] = temp; temp = array2[j]; array2[j] = array2[index]; array2[index] = temp; j -= igap; index = j + igap; } } } svd_dsort2(igap/2,n,array1,array2); } /************************************************************** * Function interchanges two vectors * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ void svd_dswap(long n, double *dx, long incx, double *dy, long incy) { #ifdef USE_BLAS cblas_dswap(n,dx,incx,dy,incy); #else long i; double dtemp; if (n <= 0 || incx == 0 || incy == 0) return; if (incx == 1 && incy == 1) { for (i=0; i < n; i++) { dtemp = *dy; *dy++ = *dx; *dx++ = dtemp; } } else { if (incx < 0) dx += (-n+1) * incx; if (incy < 0) dy += (-n+1) * incy; for (i=0; i < n; i++) { dtemp = *dy; *dy = *dx; *dx = dtemp; dx += incx; dy += incy; } } #endif } /***************************************************************** * Function finds the index of element having max. absolute value* * based on FORTRAN 77 routine from Linpack by J. Dongarra * *****************************************************************/ long svd_idamax(long n, double *dx, long incx) { long ix,i,imax; #ifdef USE_BLAS imax=cblas_idamax(n,dx,incx); #else double dtemp, dmax; if (n < 1) return(-1); if (n == 1) return(0); if (incx == 0) return(-1); if (incx < 0) ix = (-n+1) * incx; else ix = 0; imax = ix; dx += ix; dmax = fabs(*dx); for (i=1; i < n; i++) { ix += incx; dx += incx; dtemp = fabs(*dx); if (dtemp > dmax) { dmax = dtemp; imax = ix; } } #endif return(imax); } /************************************************************** * multiplication of matrix B by vector x, where B = A'A, * * and A is nrow by ncol (nrow >> ncol). Hence, B is of order * * n = ncol (y stores product vector). * **************************************************************/ void svd_opb(SMat A, double *x, double *y, double *temp) { long i, j, end; long *pointr = A->pointr, *rowind = A->rowind; double *value = A->value; long n = A->cols; SVDCount[SVD_MXV] += 2; memset(y, 0, n * sizeof(double)); for (i = 0; i < A->rows; i++) temp[i] = 0.0; for (i = 0; i < A->cols; i++) { end = pointr[i+1]; for (j = pointr[i]; j < end; j++) temp[rowind[j]] += value[j] * (*x); x++; } for (i = 0; i < A->cols; i++) { end = pointr[i+1]; for (j = pointr[i]; j < end; j++) *y += value[j] * temp[rowind[j]]; y++; } return; } /*********************************************************** * multiplication of matrix A by vector x, where A is * * nrow by ncol (nrow >> ncol). y stores product vector. * ***********************************************************/ void svd_opa(SMat A, double *x, double *y) { long end, i, j; long *pointr = A->pointr, *rowind = A->rowind; double *value = A->value; SVDCount[SVD_MXV]++; memset(y, 0, A->rows * sizeof(double)); for (i = 0; i < A->cols; i++) { end = pointr[i+1]; for (j = pointr[i]; j < end; j++) y[rowind[j]] += value[j] * x[i]; } return; } /*********************************************************************** * * * random() * * (double precision) * ***********************************************************************/ /*********************************************************************** Description ----------- This is a translation of a Fortran-77 uniform random number generator. The code is based on theory and suggestions given in D. E. Knuth (1969), vol 2. The argument to the function should be initialized to an arbitrary integer prior to the first call to random. The calling program should not alter the value of the argument between subsequent calls to random. Random returns values within the interval (0,1). Arguments --------- (input) iy an integer seed whose value must not be altered by the caller between subsequent calls (output) random a double precision random number between (0,1) ***********************************************************************/ double svd_random2(long *iy) { static long m2 = 0; static long ia, ic, mic; static double halfm, s; /* If first entry, compute (max int) / 2 */ if (!m2) { m2 = 1 << (8 * (int)sizeof(int) - 2); halfm = m2; /* compute multiplier and increment for linear congruential * method */ ia = 8 * (long)(halfm * atan(1.0) / 8.0) + 5; ic = 2 * (long)(halfm * (0.5 - sqrt(3.0)/6.0)) + 1; mic = (m2-ic) + m2; /* s is the scale factor for converting to floating point */ s = 0.5 / halfm; } /* compute next random number */ *iy = *iy * ia; /* for computers which do not allow integer overflow on addition */ if (*iy > mic) *iy = (*iy - m2) - m2; *iy = *iy + ic; /* for computers whose word length for addition is greater than * for multiplication */ if (*iy / 2 > m2) *iy = (*iy - m2) - m2; /* for computers whose integer overflow affects the sign bit */ if (*iy < 0) *iy = (*iy + m2) + m2; return((double)(*iy) * s); } /************************************************************** * * * Function finds sqrt(a^2 + b^2) without overflow or * * destructive underflow. * * * **************************************************************/ /************************************************************** Funtions used ------------- UTILITY dmax, dmin **************************************************************/ double svd_pythag(double a, double b) { double p, r, s, t, u, temp; p = svd_dmax(fabs(a), fabs(b)); if (p != 0.0) { temp = svd_dmin(fabs(a), fabs(b)) / p; r = temp * temp; t = 4.0 + r; while (t != 4.0) { s = r / t; u = 1.0 + 2.0 * s; p *= u; temp = s / u; r *= temp * temp; t = 4.0 + r; } } return(p); } ================================================ FILE: GSLAM/svdutil.hpp ================================================ /* Copyright © 2002, University of Tennessee Research Foundation. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the University of Tennessee nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #ifndef SVDUTIL_H #define SVDUTIL_H #include "svdlib.hpp" #include #define SAFE_FREE(a) {if (a) {free(a); a = NULL;}} #define USE_BLAS /* Allocates an array of longs. */ extern long *svd_longArray(long size, char empty, char *name); /* Allocates an array of doubles. */ extern double *svd_doubleArray(long size, char empty, char *name); extern void svd_debug(char *fmt, ...); extern void svd_error(char *fmt, ...); extern void svd_fatalError(char *fmt, ...); extern FILE *svd_fatalReadFile(char *filename); extern FILE *svd_readFile(char *fileName); extern FILE *svd_writeFile(char *fileName, char append); extern void svd_closeFile(FILE *file); extern char svd_readBinInt(FILE *file, int *val); extern char svd_readBinFloat(FILE *file, float *val); extern char svd_writeBinInt(FILE *file, int x); extern char svd_writeBinFloat(FILE *file, float r); /************************************************************** * returns |a| if b is positive; else fsign returns -|a| * **************************************************************/ extern double svd_fsign(double a, double b); /************************************************************** * returns the larger of two double precision numbers * **************************************************************/ extern double svd_dmax(double a, double b); /************************************************************** * returns the smaller of two double precision numbers * **************************************************************/ extern double svd_dmin(double a, double b); /************************************************************** * returns the larger of two integers * **************************************************************/ extern long svd_imax(long a, long b); /************************************************************** * returns the smaller of two integers * **************************************************************/ extern long svd_imin(long a, long b); /************************************************************** * Function scales a vector by a constant. * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ extern void svd_dscal(long n, double da, double *dx, long incx); //#define svd_dscal cblas_dscal /************************************************************** * function scales a vector by a constant. * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ extern void svd_datx(long n, double da, double *dx, long incx, double *dy, long incy); //#define svd_datx cblas_datx /************************************************************** * Function copies a vector x to a vector y * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ extern void svd_dcopy(long n, double *dx, long incx, double *dy, long incy); /************************************************************** * Function forms the dot product of two vectors. * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ extern double svd_ddot(long n, double *dx, long incx, double *dy, long incy); /************************************************************** * Constant times a vector plus a vector * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ extern void svd_daxpy (long n, double da, double *dx, long incx, double *dy, long incy); /********************************************************************* * Function sorts array1 and array2 into increasing order for array1 * *********************************************************************/ extern void svd_dsort2(long igap, long n, double *array1, double *array2); /************************************************************** * Function interchanges two vectors * * Based on Fortran-77 routine from Linpack by J. Dongarra * **************************************************************/ extern void svd_dswap(long n, double *dx, long incx, double *dy, long incy); /***************************************************************** * Function finds the index of element having max. absolute value* * based on FORTRAN 77 routine from Linpack by J. Dongarra * *****************************************************************/ extern long svd_idamax(long n, double *dx, long incx); /************************************************************** * multiplication of matrix B by vector x, where B = A'A, * * and A is nrow by ncol (nrow >> ncol). Hence, B is of order * * n = ncol (y stores product vector). * **************************************************************/ extern void svd_opb(SMat A, double *x, double *y, double *temp); /*********************************************************** * multiplication of matrix A by vector x, where A is * * nrow by ncol (nrow >> ncol). y stores product vector. * ***********************************************************/ extern void svd_opa(SMat A, double *x, double *y); /*********************************************************************** * * * random2() * * (double precision) * ***********************************************************************/ extern double svd_random2(long *iy); /************************************************************** * * * Function finds sqrt(a^2 + b^2) without overflow or * * destructive underflow. * * * **************************************************************/ extern double svd_pythag(double a, double b); #endif /* SVDUTIL_H */ ================================================ FILE: GSLAM/time_measurement.hpp ================================================ /****************************************************************************** * Author: Laurent Kneip * * Contact: kneip.laurent@gmail.com * * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. * * * * Redistribution and use in source and binary forms, with or without * * modification, are permitted provided that the following conditions * * are met: * * * Redistributions of source code must retain the above copyright * * notice, this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright * * notice, this list of conditions and the following disclaimer in the * * documentation and/or other materials provided with the distribution. * * * Neither the name of ANU nor the names of its contributors may be * * used to endorse or promote products derived from this software without * * specific prior written permission. * * * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * * SUCH DAMAGE. * ******************************************************************************/ #ifndef OPENGV_TIME_MEASUREMENT_HPP_ #define OPENGV_TIME_MEASUREMENT_HPP_ #include #include #ifdef WIN32 struct timeval { int tv_sec; int tv_usec; }; void gettimeofday( timeval * timeofday, int dummy); #else #include #endif #define TIMETODOUBLE(x) ( x.tv_sec + x.tv_usec * 1e-6 ) timeval timeval_minus( const struct timeval &t1, const struct timeval &t2 ); #endif /* OPENGV_TIME_MEASUREMENT_HPP_ */ ================================================ FILE: GSLAM.xcodeproj/project.pbxproj ================================================ // !$*UTF8*$! { archiveVersion = 1; classes = { }; objectVersion = 46; objects = { /* Begin PBXBuildFile section */ 862EB4DC1D9C9906003AABED /* GlobalReconstruction.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 862EB4DB1D9C9906003AABED /* GlobalReconstruction.cpp */; }; 863568E41D7D32E900F2EDB1 /* ORBextractor.cc in Sources */ = {isa = PBXBuildFile; fileRef = 863568E21D7D32E900F2EDB1 /* ORBextractor.cc */; }; 863568E71D7D3C9400F2EDB1 /* Accelerate.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 863568E61D7D3C9400F2EDB1 /* Accelerate.framework */; }; 863568EB1D7D3EB900F2EDB1 /* CoreVideo.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 863568EA1D7D3EB900F2EDB1 /* CoreVideo.framework */; }; 863568ED1D7D3EC700F2EDB1 /* CoreMedia.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 863568EC1D7D3EC700F2EDB1 /* CoreMedia.framework */; }; 863568EF1D7D3F0400F2EDB1 /* KeyFrame.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 863568EE1D7D3F0400F2EDB1 /* KeyFrame.cpp */; }; 863568F21D7D41CE00F2EDB1 /* Frame.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 863568F11D7D41CE00F2EDB1 /* Frame.cpp */; }; 8645FDD61D9DD5FD0077C95A /* GlobalScaleEstimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8645FDD41D9DD5FD0077C95A /* GlobalScaleEstimation.cpp */; }; 864C60C81D80015300F87039 /* ORBmatcher.cc in Sources */ = {isa = PBXBuildFile; fileRef = 864C60C61D80015300F87039 /* ORBmatcher.cc */; }; 8668D70E1D7E381500FC7DE5 /* Transform.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8668D70D1D7E381500FC7DE5 /* Transform.cpp */; }; 867170801D85164F00A8AE13 /* TrackUtil.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8671707F1D85164F00A8AE13 /* TrackUtil.cpp */; }; 867170821D851E2200A8AE13 /* Estimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 867170811D851E2200A8AE13 /* Estimation.cpp */; }; 86737C1C1D7BEBA8005AAEAC /* Foundation.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86737C1B1D7BEBA8005AAEAC /* Foundation.framework */; }; 86737C1E1D7BEBB4005AAEAC /* CoreFoundation.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86737C1D1D7BEBB4005AAEAC /* CoreFoundation.framework */; }; 86737C211D7BF63F005AAEAC /* System.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86737C201D7BF63F005AAEAC /* System.cpp */; }; 86737C291D7C0513005AAEAC /* KeyFrameDatabase.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86737C271D7C0513005AAEAC /* KeyFrameDatabase.cpp */; }; 8674FBB41D76587800FB83D1 /* main.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8674FBB31D76587800FB83D1 /* main.cpp */; }; 867F57501D823A5200CF0965 /* KLTUtil.h in Sources */ = {isa = PBXBuildFile; fileRef = 867F574F1D823A5200CF0965 /* KLTUtil.h */; }; 868033AE1D81497B003622CA /* convolve.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 868033AD1D81497B003622CA /* convolve.cpp */; }; 868033B01D814A73003622CA /* KLTUtil.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 868033AF1D814A73003622CA /* KLTUtil.cpp */; }; 86A145A31DA3293000340813 /* GlobalTranslationEstimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86A145A21DA3293000340813 /* GlobalTranslationEstimation.cpp */; }; 86A1E4341D9206A1003EB022 /* MapPoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86A1E4331D9206A1003EB022 /* MapPoint.cpp */; }; 86A7B3F31D81EBEC004C0F28 /* pyramid.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86A7B3F11D81EBEC004C0F28 /* pyramid.cpp */; }; 86A8C9011DE0275C008F117C /* AVFoundation.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A8C9001DE0275C008F117C /* AVFoundation.framework */; }; 86A8C9031DE061F4008F117C /* OpenGL.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A8C9021DE061F4008F117C /* OpenGL.framework */; }; 86B4CDBA1DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86B4CDB91DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp */; }; 86B832671D97A49200A0B460 /* KeyFrameConnection.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86B832651D97A49200A0B460 /* KeyFrameConnection.cpp */; }; 86BA29661D86A793003EBBDE /* las2.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86BA29611D86A793003EBBDE /* las2.cpp */; }; 86BA29671D86A793003EBBDE /* svdlib.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86BA29621D86A793003EBBDE /* svdlib.cpp */; }; 86BA29681D86A793003EBBDE /* svdutil.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86BA29641D86A793003EBBDE /* svdutil.cpp */; }; 86E4B7B21D8E42F200058234 /* Geometry.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86E4B7B11D8E42F200058234 /* Geometry.cpp */; }; 86F47B4C1D80E2E600F47ACF /* error.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86F47B451D80E2E600F47ACF /* error.cpp */; }; C4191123209344AF000BE72F /* libtheia.a in Frameworks */ = {isa = PBXBuildFile; fileRef = C4191122209344AF000BE72F /* libtheia.a */; }; /* End PBXBuildFile section */ /* Begin PBXCopyFilesBuildPhase section */ 8674FBAE1D76587800FB83D1 /* CopyFiles */ = { isa = PBXCopyFilesBuildPhase; buildActionMask = 2147483647; dstPath = /usr/share/man/man1/; dstSubfolderSpec = 0; files = ( ); runOnlyForDeploymentPostprocessing = 1; }; /* End PBXCopyFilesBuildPhase section */ /* Begin PBXFileReference section */ 862EB4DB1D9C9906003AABED /* GlobalReconstruction.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalReconstruction.cpp; sourceTree = ""; }; 863568E21D7D32E900F2EDB1 /* ORBextractor.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = ORBextractor.cc; sourceTree = ""; }; 863568E31D7D32E900F2EDB1 /* ORBextractor.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ORBextractor.h; sourceTree = ""; }; 863568E51D7D37E200F2EDB1 /* MapPoint.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = MapPoint.h; sourceTree = ""; }; 863568E61D7D3C9400F2EDB1 /* Accelerate.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = Accelerate.framework; path = System/Library/Frameworks/Accelerate.framework; sourceTree = SDKROOT; }; 863568EA1D7D3EB900F2EDB1 /* CoreVideo.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreVideo.framework; path = System/Library/Frameworks/CoreVideo.framework; sourceTree = SDKROOT; }; 863568EC1D7D3EC700F2EDB1 /* CoreMedia.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreMedia.framework; path = System/Library/Frameworks/CoreMedia.framework; sourceTree = SDKROOT; }; 863568EE1D7D3F0400F2EDB1 /* KeyFrame.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KeyFrame.cpp; sourceTree = ""; }; 863568F11D7D41CE00F2EDB1 /* Frame.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Frame.cpp; sourceTree = ""; }; 863568F31D7D425F00F2EDB1 /* Settings.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Settings.h; sourceTree = ""; }; 863568F41D7D436500F2EDB1 /* Estimation.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Estimation.h; sourceTree = ""; }; 863568F61D7D5E0B00F2EDB1 /* Transform.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Transform.h; sourceTree = ""; }; 8645FDD41D9DD5FD0077C95A /* GlobalScaleEstimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalScaleEstimation.cpp; sourceTree = ""; }; 864C60C61D80015300F87039 /* ORBmatcher.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = ORBmatcher.cc; sourceTree = ""; }; 864C60C71D80015300F87039 /* ORBmatcher.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ORBmatcher.h; sourceTree = ""; }; 865F271D1D88D8F9002100FD /* selectGoodFeatures.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = selectGoodFeatures.hpp; sourceTree = ""; }; 8668D70D1D7E381500FC7DE5 /* Transform.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Transform.cpp; sourceTree = ""; }; 8671707E1D85163300A8AE13 /* TrackUtil.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = TrackUtil.h; sourceTree = ""; }; 8671707F1D85164F00A8AE13 /* TrackUtil.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = TrackUtil.cpp; sourceTree = ""; }; 867170811D851E2200A8AE13 /* Estimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Estimation.cpp; sourceTree = ""; }; 86737C1B1D7BEBA8005AAEAC /* Foundation.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = Foundation.framework; path = System/Library/Frameworks/Foundation.framework; sourceTree = SDKROOT; }; 86737C1D1D7BEBB4005AAEAC /* CoreFoundation.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreFoundation.framework; path = System/Library/Frameworks/CoreFoundation.framework; sourceTree = SDKROOT; }; 86737C1F1D7BF625005AAEAC /* System.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = System.h; sourceTree = ""; }; 86737C201D7BF63F005AAEAC /* System.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = System.cpp; sourceTree = ""; }; 86737C221D7BFDB4005AAEAC /* ORBVocabulary.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ORBVocabulary.h; sourceTree = ""; }; 86737C261D7C043D005AAEAC /* DBoW2 */ = {isa = PBXFileReference; lastKnownFileType = folder; path = DBoW2; sourceTree = ""; }; 86737C271D7C0513005AAEAC /* KeyFrameDatabase.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KeyFrameDatabase.cpp; sourceTree = ""; }; 86737C281D7C0513005AAEAC /* KeyFrameDatabase.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KeyFrameDatabase.h; sourceTree = ""; }; 86737C2A1D7C05E1005AAEAC /* Frame.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = Frame.h; sourceTree = ""; }; 86737C2B1D7C05E1005AAEAC /* KeyFrame.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KeyFrame.h; sourceTree = ""; }; 8674FBB01D76587800FB83D1 /* GSLAM */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = GSLAM; sourceTree = BUILT_PRODUCTS_DIR; }; 8674FBB31D76587800FB83D1 /* main.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = main.cpp; sourceTree = ""; }; 867F574F1D823A5200CF0965 /* KLTUtil.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KLTUtil.h; sourceTree = ""; }; 868033AC1D8148A0003622CA /* convolve.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = convolve.h; sourceTree = ""; }; 868033AD1D81497B003622CA /* convolve.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = convolve.cpp; sourceTree = ""; }; 868033AF1D814A73003622CA /* KLTUtil.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KLTUtil.cpp; sourceTree = ""; }; 86A145A21DA3293000340813 /* GlobalTranslationEstimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalTranslationEstimation.cpp; sourceTree = ""; }; 86A1E4331D9206A1003EB022 /* MapPoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = MapPoint.cpp; sourceTree = ""; }; 86A7B3EF1D81D9E5004C0F28 /* GlobalReconstruction.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = GlobalReconstruction.h; sourceTree = ""; }; 86A7B3F01D81E591004C0F28 /* pyramid.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = pyramid.h; sourceTree = ""; }; 86A7B3F11D81EBEC004C0F28 /* pyramid.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = pyramid.cpp; sourceTree = ""; }; 86A8C9001DE0275C008F117C /* AVFoundation.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = AVFoundation.framework; path = System/Library/Frameworks/AVFoundation.framework; sourceTree = SDKROOT; }; 86A8C9021DE061F4008F117C /* OpenGL.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = OpenGL.framework; path = System/Library/Frameworks/OpenGL.framework; sourceTree = SDKROOT; }; 86A8C9041DE0646F008F117C /* Drawer.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Drawer.h; sourceTree = ""; }; 86A8C9061DE06768008F117C /* Drawer.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = Drawer.cpp; sourceTree = ""; }; 86B4CDB91DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalRotationEstimation.cpp; sourceTree = ""; }; 86B832651D97A49200A0B460 /* KeyFrameConnection.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KeyFrameConnection.cpp; sourceTree = ""; }; 86B832661D97A49200A0B460 /* KeyFrameConnection.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KeyFrameConnection.h; sourceTree = ""; }; 86BA29611D86A793003EBBDE /* las2.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = las2.cpp; sourceTree = ""; }; 86BA29621D86A793003EBBDE /* svdlib.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = svdlib.cpp; sourceTree = ""; }; 86BA29631D86A793003EBBDE /* svdlib.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = svdlib.hpp; sourceTree = ""; }; 86BA29641D86A793003EBBDE /* svdutil.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = svdutil.cpp; sourceTree = ""; }; 86BA29651D86A793003EBBDE /* svdutil.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = svdutil.hpp; sourceTree = ""; }; 86BA296B1D86A867003EBBDE /* ImageGrids.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = ImageGrids.hpp; sourceTree = ""; }; 86BA296C1D86A867003EBBDE /* IMU.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = IMU.hpp; sourceTree = ""; }; 86E4B7B01D8E3D4300058234 /* Geometry.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Geometry.h; sourceTree = ""; }; 86E4B7B11D8E42F200058234 /* Geometry.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Geometry.cpp; sourceTree = ""; }; 86EA059D1DA1B5FD00A32616 /* L1Solver.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = L1Solver.h; sourceTree = ""; }; 86F47B451D80E2E600F47ACF /* error.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = error.cpp; sourceTree = ""; }; 86F47B461D80E2E600F47ACF /* error.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = error.h; sourceTree = ""; }; 86F47B481D80E2E600F47ACF /* KLT.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KLT.h; sourceTree = ""; }; 86F47B4B1D80E2E600F47ACF /* LK.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = LK.hpp; sourceTree = ""; }; C4191122209344AF000BE72F /* libtheia.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libtheia.a; path = GSLAM/lib/libtheia.a; sourceTree = ""; }; C419112420934AD8000BE72F /* DUtils */ = {isa = PBXFileReference; lastKnownFileType = folder; path = DUtils; sourceTree = ""; }; C4B124381E51504300670AFD /* Homography.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = Homography.hpp; sourceTree = ""; }; C4B124391E57C44B00670AFD /* RelativeMotion.hpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.h; path = RelativeMotion.hpp; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ 8674FBAD1D76587800FB83D1 /* Frameworks */ = { isa = PBXFrameworksBuildPhase; buildActionMask = 2147483647; files = ( 86A8C9031DE061F4008F117C /* OpenGL.framework in Frameworks */, 86A8C9011DE0275C008F117C /* AVFoundation.framework in Frameworks */, 863568ED1D7D3EC700F2EDB1 /* CoreMedia.framework in Frameworks */, C4191123209344AF000BE72F /* libtheia.a in Frameworks */, 863568EB1D7D3EB900F2EDB1 /* CoreVideo.framework in Frameworks */, 863568E71D7D3C9400F2EDB1 /* Accelerate.framework in Frameworks */, 86737C1E1D7BEBB4005AAEAC /* CoreFoundation.framework in Frameworks */, 86737C1C1D7BEBA8005AAEAC /* Foundation.framework in Frameworks */, ); runOnlyForDeploymentPostprocessing = 0; }; /* End PBXFrameworksBuildPhase section */ /* Begin PBXGroup section */ 863568E81D7D3D4900F2EDB1 /* ORB */ = { isa = PBXGroup; children = ( 864C60C61D80015300F87039 /* ORBmatcher.cc */, 864C60C71D80015300F87039 /* ORBmatcher.h */, 863568E21D7D32E900F2EDB1 /* ORBextractor.cc */, 863568E31D7D32E900F2EDB1 /* ORBextractor.h */, 86737C221D7BFDB4005AAEAC /* ORBVocabulary.h */, ); name = ORB; sourceTree = ""; }; 863568E91D7D3DB400F2EDB1 /* Frame */ = { isa = PBXGroup; children = ( 86737C2A1D7C05E1005AAEAC /* Frame.h */, 863568F11D7D41CE00F2EDB1 /* Frame.cpp */, 86737C2B1D7C05E1005AAEAC /* KeyFrame.h */, 863568EE1D7D3F0400F2EDB1 /* KeyFrame.cpp */, 86B832651D97A49200A0B460 /* KeyFrameConnection.cpp */, 86B832661D97A49200A0B460 /* KeyFrameConnection.h */, 86737C281D7C0513005AAEAC /* KeyFrameDatabase.h */, 86737C271D7C0513005AAEAC /* KeyFrameDatabase.cpp */, ); name = Frame; sourceTree = ""; }; 863568F01D7D3F2700F2EDB1 /* Geometry */ = { isa = PBXGroup; children = ( 863568E51D7D37E200F2EDB1 /* MapPoint.h */, 86A1E4331D9206A1003EB022 /* MapPoint.cpp */, 867170811D851E2200A8AE13 /* Estimation.cpp */, 863568F41D7D436500F2EDB1 /* Estimation.h */, 863568F61D7D5E0B00F2EDB1 /* Transform.h */, 8668D70D1D7E381500FC7DE5 /* Transform.cpp */, 86EA059D1DA1B5FD00A32616 /* L1Solver.h */, 86A7B3EF1D81D9E5004C0F28 /* GlobalReconstruction.h */, 862EB4DB1D9C9906003AABED /* GlobalReconstruction.cpp */, 86A145A21DA3293000340813 /* GlobalTranslationEstimation.cpp */, 86B4CDB91DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp */, 8645FDD41D9DD5FD0077C95A /* GlobalScaleEstimation.cpp */, 86E4B7B01D8E3D4300058234 /* Geometry.h */, 86E4B7B11D8E42F200058234 /* Geometry.cpp */, C4B124391E57C44B00670AFD /* RelativeMotion.hpp */, ); name = Geometry; sourceTree = ""; }; 863568F51D7D562800F2EDB1 /* Tracking */ = { isa = PBXGroup; children = ( C4B124381E51504300670AFD /* Homography.hpp */, 865F271D1D88D8F9002100FD /* selectGoodFeatures.hpp */, 868033AC1D8148A0003622CA /* convolve.h */, 868033AD1D81497B003622CA /* convolve.cpp */, 86F47B451D80E2E600F47ACF /* error.cpp */, 86F47B461D80E2E600F47ACF /* error.h */, 86F47B481D80E2E600F47ACF /* KLT.h */, 867F574F1D823A5200CF0965 /* KLTUtil.h */, 868033AF1D814A73003622CA /* KLTUtil.cpp */, 86A7B3F01D81E591004C0F28 /* pyramid.h */, 86A7B3F11D81EBEC004C0F28 /* pyramid.cpp */, 86F47B4B1D80E2E600F47ACF /* LK.hpp */, 8671707E1D85163300A8AE13 /* TrackUtil.h */, 8671707F1D85164F00A8AE13 /* TrackUtil.cpp */, ); name = Tracking; sourceTree = ""; }; 8674FBA71D76587800FB83D1 = { isa = PBXGroup; children = ( C4191122209344AF000BE72F /* libtheia.a */, 86A8C9021DE061F4008F117C /* OpenGL.framework */, 86A8C9001DE0275C008F117C /* AVFoundation.framework */, 863568EC1D7D3EC700F2EDB1 /* CoreMedia.framework */, 863568EA1D7D3EB900F2EDB1 /* CoreVideo.framework */, 863568E61D7D3C9400F2EDB1 /* Accelerate.framework */, 86737C1D1D7BEBB4005AAEAC /* CoreFoundation.framework */, 86737C1B1D7BEBA8005AAEAC /* Foundation.framework */, 8674FBB21D76587800FB83D1 /* GSLAM */, 8674FBB11D76587800FB83D1 /* Products */, ); sourceTree = ""; }; 8674FBB11D76587800FB83D1 /* Products */ = { isa = PBXGroup; children = ( 8674FBB01D76587800FB83D1 /* GSLAM */, ); name = Products; sourceTree = ""; }; 8674FBB21D76587800FB83D1 /* GSLAM */ = { isa = PBXGroup; children = ( 86A8C9051DE06478008F117C /* GUI */, 86BA296A1D86A82F003EBBDE /* IMU */, 86BA29691D86A79E003EBBDE /* svd */, 863568F51D7D562800F2EDB1 /* Tracking */, 863568F01D7D3F2700F2EDB1 /* Geometry */, 863568E91D7D3DB400F2EDB1 /* Frame */, 863568E81D7D3D4900F2EDB1 /* ORB */, C419112420934AD8000BE72F /* DUtils */, 86737C261D7C043D005AAEAC /* DBoW2 */, 863568F31D7D425F00F2EDB1 /* Settings.h */, 86737C1F1D7BF625005AAEAC /* System.h */, 86737C201D7BF63F005AAEAC /* System.cpp */, 8674FBB31D76587800FB83D1 /* main.cpp */, ); path = GSLAM; sourceTree = ""; }; 86A8C9051DE06478008F117C /* GUI */ = { isa = PBXGroup; children = ( 86A8C9041DE0646F008F117C /* Drawer.h */, 86A8C9061DE06768008F117C /* Drawer.cpp */, ); name = GUI; sourceTree = ""; }; 86BA29691D86A79E003EBBDE /* svd */ = { isa = PBXGroup; children = ( 86BA29611D86A793003EBBDE /* las2.cpp */, 86BA29621D86A793003EBBDE /* svdlib.cpp */, 86BA29631D86A793003EBBDE /* svdlib.hpp */, 86BA29641D86A793003EBBDE /* svdutil.cpp */, 86BA29651D86A793003EBBDE /* svdutil.hpp */, ); name = svd; sourceTree = ""; }; 86BA296A1D86A82F003EBBDE /* IMU */ = { isa = PBXGroup; children = ( 86BA296B1D86A867003EBBDE /* ImageGrids.hpp */, 86BA296C1D86A867003EBBDE /* IMU.hpp */, ); name = IMU; sourceTree = ""; }; /* End PBXGroup section */ /* Begin PBXNativeTarget section */ 8674FBAF1D76587800FB83D1 /* GSLAM */ = { isa = PBXNativeTarget; buildConfigurationList = 8674FBB71D76587800FB83D1 /* Build configuration list for PBXNativeTarget "GSLAM" */; buildPhases = ( 8674FBAC1D76587800FB83D1 /* Sources */, 8674FBAD1D76587800FB83D1 /* Frameworks */, 8674FBAE1D76587800FB83D1 /* CopyFiles */, ); buildRules = ( ); dependencies = ( ); name = GSLAM; productName = GSLAM; productReference = 8674FBB01D76587800FB83D1 /* GSLAM */; productType = "com.apple.product-type.tool"; }; /* End PBXNativeTarget section */ /* Begin PBXProject section */ 8674FBA81D76587800FB83D1 /* Project object */ = { isa = PBXProject; attributes = { LastUpgradeCheck = 0730; ORGANIZATIONNAME = ctang; TargetAttributes = { 8674FBAF1D76587800FB83D1 = { CreatedOnToolsVersion = 7.3.1; }; }; }; buildConfigurationList = 8674FBAB1D76587800FB83D1 /* Build configuration list for PBXProject "GSLAM" */; compatibilityVersion = "Xcode 3.2"; developmentRegion = English; hasScannedForEncodings = 0; knownRegions = ( en, ); mainGroup = 8674FBA71D76587800FB83D1; productRefGroup = 8674FBB11D76587800FB83D1 /* Products */; projectDirPath = ""; projectRoot = ""; targets = ( 8674FBAF1D76587800FB83D1 /* GSLAM */, ); }; /* End PBXProject section */ /* Begin PBXSourcesBuildPhase section */ 8674FBAC1D76587800FB83D1 /* Sources */ = { isa = PBXSourcesBuildPhase; buildActionMask = 2147483647; files = ( 86A7B3F31D81EBEC004C0F28 /* pyramid.cpp in Sources */, 86BA29681D86A793003EBBDE /* svdutil.cpp in Sources */, 868033AE1D81497B003622CA /* convolve.cpp in Sources */, 8668D70E1D7E381500FC7DE5 /* Transform.cpp in Sources */, 8645FDD61D9DD5FD0077C95A /* GlobalScaleEstimation.cpp in Sources */, 86737C211D7BF63F005AAEAC /* System.cpp in Sources */, 86B832671D97A49200A0B460 /* KeyFrameConnection.cpp in Sources */, 86A1E4341D9206A1003EB022 /* MapPoint.cpp in Sources */, 868033B01D814A73003622CA /* KLTUtil.cpp in Sources */, 86737C291D7C0513005AAEAC /* KeyFrameDatabase.cpp in Sources */, 86F47B4C1D80E2E600F47ACF /* error.cpp in Sources */, 86B4CDBA1DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp in Sources */, 863568EF1D7D3F0400F2EDB1 /* KeyFrame.cpp in Sources */, 867170801D85164F00A8AE13 /* TrackUtil.cpp in Sources */, 86BA29671D86A793003EBBDE /* svdlib.cpp in Sources */, 864C60C81D80015300F87039 /* ORBmatcher.cc in Sources */, 863568F21D7D41CE00F2EDB1 /* Frame.cpp in Sources */, 86BA29661D86A793003EBBDE /* las2.cpp in Sources */, 86E4B7B21D8E42F200058234 /* Geometry.cpp in Sources */, 867170821D851E2200A8AE13 /* Estimation.cpp in Sources */, 867F57501D823A5200CF0965 /* KLTUtil.h in Sources */, 863568E41D7D32E900F2EDB1 /* ORBextractor.cc in Sources */, 86A145A31DA3293000340813 /* GlobalTranslationEstimation.cpp in Sources */, 862EB4DC1D9C9906003AABED /* GlobalReconstruction.cpp in Sources */, 8674FBB41D76587800FB83D1 /* main.cpp in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; /* End PBXSourcesBuildPhase section */ /* Begin XCBuildConfiguration section */ 8674FBB51D76587800FB83D1 /* Debug */ = { isa = XCBuildConfiguration; buildSettings = { ALWAYS_SEARCH_USER_PATHS = NO; CLANG_ANALYZER_NONNULL = YES; CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x"; CLANG_CXX_LIBRARY = "libc++"; CLANG_ENABLE_MODULES = YES; CLANG_ENABLE_OBJC_ARC = YES; CLANG_WARN_BOOL_CONVERSION = YES; CLANG_WARN_CONSTANT_CONVERSION = YES; CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR; CLANG_WARN_EMPTY_BODY = YES; CLANG_WARN_ENUM_CONVERSION = YES; CLANG_WARN_INT_CONVERSION = YES; CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR; CLANG_WARN_UNREACHABLE_CODE = YES; CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; CODE_SIGN_IDENTITY = "-"; COPY_PHASE_STRIP = NO; DEBUG_INFORMATION_FORMAT = dwarf; ENABLE_STRICT_OBJC_MSGSEND = YES; ENABLE_TESTABILITY = YES; GCC_C_LANGUAGE_STANDARD = gnu99; GCC_DYNAMIC_NO_PIC = NO; GCC_NO_COMMON_BLOCKS = YES; GCC_OPTIMIZATION_LEVEL = 0; GCC_PREPROCESSOR_DEFINITIONS = ( "DEBUG=1", "$(inherited)", ); GCC_WARN_64_TO_32_BIT_CONVERSION = YES; GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR; GCC_WARN_UNDECLARED_SELECTOR = YES; GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; GCC_WARN_UNUSED_FUNCTION = YES; GCC_WARN_UNUSED_VARIABLE = YES; MACOSX_DEPLOYMENT_TARGET = 10.11; MTL_ENABLE_DEBUG_INFO = YES; ONLY_ACTIVE_ARCH = YES; SDKROOT = macosx; }; name = Debug; }; 8674FBB61D76587800FB83D1 /* Release */ = { isa = XCBuildConfiguration; buildSettings = { ALWAYS_SEARCH_USER_PATHS = NO; CLANG_ANALYZER_NONNULL = YES; CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x"; CLANG_CXX_LIBRARY = "libc++"; CLANG_ENABLE_MODULES = YES; CLANG_ENABLE_OBJC_ARC = YES; CLANG_WARN_BOOL_CONVERSION = YES; CLANG_WARN_CONSTANT_CONVERSION = YES; CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR; CLANG_WARN_EMPTY_BODY = YES; CLANG_WARN_ENUM_CONVERSION = YES; CLANG_WARN_INT_CONVERSION = YES; CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR; CLANG_WARN_UNREACHABLE_CODE = YES; CLANG_WARN__DUPLICATE_METHOD_MATCH = YES; CODE_SIGN_IDENTITY = "-"; COPY_PHASE_STRIP = NO; DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; ENABLE_NS_ASSERTIONS = NO; ENABLE_STRICT_OBJC_MSGSEND = YES; GCC_C_LANGUAGE_STANDARD = gnu99; GCC_NO_COMMON_BLOCKS = YES; GCC_WARN_64_TO_32_BIT_CONVERSION = YES; GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR; GCC_WARN_UNDECLARED_SELECTOR = YES; GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; GCC_WARN_UNUSED_FUNCTION = YES; GCC_WARN_UNUSED_VARIABLE = YES; MACOSX_DEPLOYMENT_TARGET = 10.11; MTL_ENABLE_DEBUG_INFO = NO; SDKROOT = macosx; }; name = Release; }; 8674FBB81D76587800FB83D1 /* Debug */ = { isa = XCBuildConfiguration; buildSettings = { ARCHS = "$(ARCHS_STANDARD_64_BIT)"; CLANG_CXX_LANGUAGE_STANDARD = "c++0x"; CLANG_CXX_LIBRARY = "libc++"; CLANG_ENABLE_OBJC_ARC = YES; CLANG_X86_VECTOR_INSTRUCTIONS = avx; GCC_C_LANGUAGE_STANDARD = c11; GCC_INPUT_FILETYPE = sourcecode.cpp.objcpp; "HEADER_SEARCH_PATHS[arch=*]" = ( /usr/local/include, /usr/local/include/eigen3, ); LD_RUNPATH_SEARCH_PATHS = ""; LIBRARY_SEARCH_PATHS = ( "$(inherited)", "$(PROJECT_DIR)/GSLAM/lib", /usr/local/lib, "$(PROJECT_DIR)", "$(PROJECT_DIR)/GSLAM", ); OTHER_LDFLAGS = ""; PRODUCT_NAME = "$(TARGET_NAME)"; SDKROOT = macosx10.13; }; name = Debug; }; 8674FBB91D76587800FB83D1 /* Release */ = { isa = XCBuildConfiguration; buildSettings = { ARCHS = "$(ARCHS_STANDARD_64_BIT)"; CLANG_CXX_LANGUAGE_STANDARD = "c++0x"; CLANG_CXX_LIBRARY = "libc++"; CLANG_ENABLE_OBJC_ARC = YES; CLANG_X86_VECTOR_INSTRUCTIONS = avx; ENABLE_NS_ASSERTIONS = YES; GCC_C_LANGUAGE_STANDARD = c11; GCC_INPUT_FILETYPE = sourcecode.cpp.objcpp; GCC_OPTIMIZATION_LEVEL = 3; "HEADER_SEARCH_PATHS[arch=*]" = ( /usr/local/include, /usr/local/include/eigen3, ); LD_RUNPATH_SEARCH_PATHS = ""; LIBRARY_SEARCH_PATHS = ( "$(inherited)", "$(PROJECT_DIR)/GSLAM/lib", /usr/local/lib, "$(PROJECT_DIR)", "$(PROJECT_DIR)/GSLAM", ); ONLY_ACTIVE_ARCH = YES; OTHER_LDFLAGS = ""; "OTHER_LDFLAGS[arch=*]" = ( "-lopencv_core", "-lglog", "-lceres", "-lopencv_calib3d", "-lopencv_imgproc", "-ltbb", "-lopencv_features2d", "-lClp", "-lCoinUtils", "-lopencv_imgcodecs", "-lpangolin", "-lopencv_highgui", "-lopencv_videoio", "-lopencv_video", "-lcholmod", "-lDBoW2", ); PRODUCT_NAME = "$(TARGET_NAME)"; SDKROOT = macosx10.13; }; name = Release; }; /* End XCBuildConfiguration section */ /* Begin XCConfigurationList section */ 8674FBAB1D76587800FB83D1 /* Build configuration list for PBXProject "GSLAM" */ = { isa = XCConfigurationList; buildConfigurations = ( 8674FBB51D76587800FB83D1 /* Debug */, 8674FBB61D76587800FB83D1 /* Release */, ); defaultConfigurationIsVisible = 0; defaultConfigurationName = Release; }; 8674FBB71D76587800FB83D1 /* Build configuration list for PBXNativeTarget "GSLAM" */ = { isa = XCConfigurationList; buildConfigurations = ( 8674FBB81D76587800FB83D1 /* Debug */, 8674FBB91D76587800FB83D1 /* Release */, ); defaultConfigurationIsVisible = 0; defaultConfigurationName = Release; }; /* End XCConfigurationList section */ }; rootObject = 8674FBA81D76587800FB83D1 /* Project object */; } ================================================ FILE: GSLAM.xcodeproj/project.xcworkspace/contents.xcworkspacedata ================================================ ================================================ FILE: GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist ================================================ ================================================ FILE: GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcschemes/GSLAM.xcscheme ================================================ ================================================ FILE: GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcschemes/xcschememanagement.plist ================================================ SchemeUserState GSLAM.xcscheme orderHint 0 SuppressBuildableAutocreation 8674FBAF1D76587800FB83D1 primary ================================================ FILE: GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist ================================================ ================================================ FILE: GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcschemes/GSLAM.xcscheme ================================================ ================================================ FILE: GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcschemes/xcschememanagement.plist ================================================ SchemeUserState GSLAM.xcscheme orderHint 0 SuppressBuildableAutocreation 8674FBAF1D76587800FB83D1 primary ================================================ FILE: README.md ================================================ # GSLAM: Initialization-robust Monocular Visual SLAM via Global Structure-from-Motion For more information see [https://frobelbest.github.io/gslam](https://frobelbest.github.io/gslam) ### 1. Related Papers * **GSLAM: Initialization-robust Monocular Visual SLAM via Global Structure-from-Motion**, *C. Tang, O. Wang, P. Tan*, In 3DV,2017 * **Global Structure-from-Motion by Similarity Averaging**, *Z. Cui, P. Tan*, In ICCV, 2015 Get two sample sequences from [Google Drive](https://drive.google.com/file/d/1TXRg2NuiySocCsIfibEqM4CW9u8Rleq2/view?usp=sharing) . ### 2. Installation git clone https://github.com/frobelbest/GSLAM.git #### 2.1 Required Dependencies ##### Theia Vision Library (required for global rotation averaging). Install from [http://www.theia-sfm.org](http://www.theia-sfm.org) ##### Ceres Solver (required for local and global bundle adjustment). Install from [http://ceres-solver.org](http://ceres-solver.org) ##### CLP (required for global scale and translation averaging). Install from [https://projects.coin-or.org/Clp](https://projects.coin-or.org/Clp) ##### OpenCV (required for image processing). Install from [https://opencv.org](https://opencv.org) ##### Pangolin (required for visualization). Install from [https://github.com/stevenlovegrove/Pangolin](https://github.com/stevenlovegrove/Pangolin) #### 2.3 Build Currently, only the xcode project is supplied. You can write your own code to compile on other platforms or wait for future update. ### 3 Usage Run on a dataset from [Google Drive](https://drive.google.com/file/d/1TXRg2NuiySocCsIfibEqM4CW9u8Rleq2/view?usp=sharing) using GSLAM [sequence_path] [vocabulary_path], for example GSLAM ./robot ./Vocabulary/ORBvoc.txt The ORB vocabulary for loop detection can be downloaded at [https://github.com/raulmur/ORB_SLAM2/tree/master/Vocabulary](https://github.com/raulmur/ORB_SLAM2/tree/master/Vocabulary) #### 3.1 Dataset Format. Under each sequnce folder you will see the following files: - `shake.mov` The input video. - `framestamp.txt` The timestamps for each frame. - `gyro.txt` The gyroscope readings recorded along with the video. - `config.yaml` The config settings. ### Notes #### Real-time KLT with AVX Acceleration Except the method proposed in the paper, this project also featured in a highly optimized KLT Tracker that can track more than 4000 points on a 1080p video in real-time. #### ToDo The main bottleneck for this project is the feature tracking, which can be further improved by the paper "Better feature tracking through subspace constraints". ### 5 License GSLAM was developed at the Simon Fraser University and Adobe. The open-source version is licensed under the GNU General Public License Version 3 (GPLv3). For commercial purposes, please contact [cta73@sfu.ca](cta73@sfu.ca) or [pingtan@sfu.ca](pingtan@sfu.ca)