[ { "path": "GSLAM/Drawer copy.h", "content": "//\n// Drawer.h\n// GSLAM\n//\n// Created by ctang on 11/19/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifndef DRAWER_H\n#define DRAWER_H\n\n#include \"pangolin/pangolin.h\"\n\nnamespace GSLAM\n{\n class KeyFrame;\n class Drawer{\n public:\n float mCameraSize;\n float mCameraLineWidth;\n float mGraphLineWidth;\n float mPointSize;\n \n int mFrameIndex;\n int mKeyFrameIndex;\n std::vector keyFrames;\n \n void drawCurrentCamera(pangolin::OpenGlMatrix &Twc);\n void getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &Twc);\n void drawKeyFrames();\n void drawPoints();\n };\n}\n\n#include \"KeyFrame.h\"\nnamespace GSLAM{\n \n void Drawer::drawCurrentCamera(pangolin::OpenGlMatrix &Twc) {\n \n const float &w = mCameraSize;\n const float h = w*0.75;\n const float z = w*0.6;\n \n glPushMatrix();\n glMultMatrixd(Twc.m);\n \n glLineWidth(mCameraLineWidth);\n glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);\n \n glBegin(GL_LINES);\n glVertex3f(0,0,0);\n glVertex3f(w,h,z);\n glVertex3f(0,0,0);\n glVertex3f(w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,h,z);\n \n glVertex3f(w,h,z);\n glVertex3f(w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(-w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(w,h,z);\n \n glVertex3f(-w,-h,z);\n glVertex3f(w,-h,z);\n glEnd();\n \n glPopMatrix();\n \n }\n void Drawer::getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M){\n \n if(mFrameIndex>=keyFrames[mKeyFrameIndex]->mvLocalFrames.back().frameId){\n mKeyFrameIndex++;\n }\n printf(\"%d %d\\n\",mFrameIndex,keyFrames[mKeyFrameIndex]->frameId);\n \n Transform pose;\n Transform pose1;\n \n Transform pose2\n if(mKeyFrameIndex<1){\n pose1=keyFrames[mKeyFrameIndex]->pose;\n pose2=keyFrames[mKeyFrameIndex]->pose;\n \n }else{\n pose1.rotation=keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.rotation\n *keyFrames[mKeyFrameIndex-1]->pose.rotation;\n \n Eigen::Vector3d translation;\n translation =keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.translation\n /keyFrames[mKeyFrameIndex-1]->scale;\n \n translation =keyFrames[mKeyFrameIndex-1]->pose.rotation.transpose()*translation;\n pose.translation+=translation;\n \n Transform pose2;\n pose2.\n }\n \n if (mFrameIndex==keyFrames[mKeyFrameIndex]->frameId) {\n \n }else if ((mFrameIndex-keyFrames[mKeyFrameIndex]->frameId)mvCloseFrames.size()) {\n \n /*int LocalIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId;\n \n pose.rotation=keyFrames[mKeyFrameIndex]->mvCloseFrames[LocalIndex].pose.rotation\n *keyFrames[mKeyFrameIndex]->pose.rotation;\n \n Eigen::Vector3d translation;\n translation =keyFrames[mKeyFrameIndex]->mvCloseFrames[LocalIndex].pose.translation\n /keyFrames[mKeyFrameIndex]->scale;\n translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation;\n pose.translation+=translation;*/\n \n }else{\n \n int localIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId-keyFrames[mKeyFrameIndex]->mvCloseFrames.size();\n pose.rotation=keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.rotation\n *keyFrames[mKeyFrameIndex]->pose.rotation;\n \n Eigen::Vector3d translation;\n translation =keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.translation\n /keyFrames[mKeyFrameIndex]->scale;\n \n translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation;\n pose.translation+=translation;\n }\n \n \n pose.rotation.transposeInPlace();\n \n M.m[0] = pose.rotation(0,0);\n M.m[1] = pose.rotation(1,0);\n M.m[2] = pose.rotation(2,0);\n M.m[3] = 0.0;\n \n M.m[4] = pose.rotation(0,1);\n M.m[5] = pose.rotation(1,1);\n M.m[6] = pose.rotation(2,1);\n M.m[7] = 0.0;\n \n M.m[8] = pose.rotation(0,2);\n M.m[9] = pose.rotation(1,2);\n M.m[10] = pose.rotation(2,2);\n M.m[11] = 0.0;\n \n M.m[12] = pose.translation(0);\n M.m[13] = pose.translation(1);\n M.m[14] = pose.translation(2);\n M.m[15] = 1.0;\n \n mFrameIndex++;\n }\n \n void Drawer::drawPoints(){\n glPointSize(4);\n glBegin(GL_POINTS);\n for(int k=0;kmvLocalMapPoints.size();i++) {\n if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){\n Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition();\n point3D/=keyFrames[k]->scale;\n point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation;\n uchar* color=keyFrames[k]->mvLocalMapPoints[i].color;\n glColor3f(color[2]*(1.0/255.0),color[1]*(1.0/255.0),color[0]*(1.0/255.0));\n glVertex3d(point3D(0),point3D(1),point3D(2));\n }\n }\n }\n glEnd();\n }\n \n void Drawer::drawKeyFrames(){\n \n const float &w = mCameraSize;\n const float h = w*0.75;\n const float z = w*0.6;\n \n for(size_t i=0; i<=mKeyFrameIndex; i++){\n \n KeyFrame* pKF = keyFrames[i];\n Transform pose=pKF->pose;\n pose.rotation.transposeInPlace();\n \n pangolin::OpenGlMatrix M;\n M.m[0] = pose.rotation(0,0);\n M.m[1] = pose.rotation(1,0);\n M.m[2] = pose.rotation(2,0);\n M.m[3] = 0.0;\n \n M.m[4] = pose.rotation(0,1);\n M.m[5] = pose.rotation(1,1);\n M.m[6] = pose.rotation(2,1);\n M.m[7] = 0.0;\n \n M.m[8] = pose.rotation(0,2);\n M.m[9] = pose.rotation(1,2);\n M.m[10] = pose.rotation(2,2);\n M.m[11] = 0.0;\n \n M.m[12] = pose.translation(0);\n M.m[13] = pose.translation(1);\n M.m[14] = pose.translation(2);\n M.m[15] = 1.0;\n \n glPushMatrix();\n glMultMatrixd(M.m);\n \n glLineWidth(mCameraLineWidth*1.5);\n glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);\n \n glBegin(GL_LINES);\n glVertex3f(0,0,0);\n glVertex3f(w,h,z);\n glVertex3f(0,0,0);\n glVertex3f(w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,h,z);\n \n glVertex3f(w,h,z);\n glVertex3f(w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(-w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(w,h,z);\n \n glVertex3f(-w,-h,z);\n glVertex3f(w,-h,z);\n glEnd();\n glPopMatrix();\n }\n \n glLineWidth(mGraphLineWidth);\n \n glBegin(GL_LINES);\n \n for(size_t k=0;k<=mKeyFrameIndex;k++)\n {\n // Covisibility Graph\n Eigen::Vector3d center=keyFrames[k]->pose.translation;\n for(map::iterator mit=keyFrames[k]->mConnectedKeyFramePoses.begin(),\n mend=keyFrames[k]->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n if(mit->first->mnIdmnId){\n if(mit->first->nextKeyFramePtr==keyFrames[k]&&mit->first==keyFrames[k]->prevKeyFramePtr){\n \n glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);\n Eigen::Vector3d center2=mit->first->pose.translation;\n glVertex3d(center(0),center(1),center(2));\n glVertex3d(center2(0),center2(1),center2(2));\n \n }else if(mit->second.scale==-1.0){\n glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0);\n Eigen::Vector3d center2=mit->first->pose.translation;\n glVertex3d(center(0),center(1),center(2));\n glVertex3d(center2(0),center2(1),center2(2));\n\n }else{\n glColor3f(237.0f/255.0,177.0f/255.0,32.0f/255.0);\n Eigen::Vector3d center2=mit->first->pose.translation;\n glVertex3d(center(0),center(1),center(2));\n glVertex3d(center2(0),center2(1),center2(2));\n }\n }\n }\n }\n glEnd();\n }\n}\n\n#endif" }, { "path": "GSLAM/Drawer.cpp", "content": "//\n// Drawer.cpp\n// GSLAM\n//\n// Created by ctang on 11/19/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n\n\n" }, { "path": "GSLAM/Drawer.h", "content": "//\n// Drawer.h\n// GSLAM\n//\n// Created by ctang on 11/19/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifndef DRAWER_H\n#define DRAWER_H\n\n#include \"pangolin/pangolin.h\"\n#include \"ceres/rotation.h\"\n#include \n#include \n\nnamespace GSLAM\n{\n class KeyFrame;\n class Drawer{\n public:\n float mCameraSize;\n float mCameraLineWidth;\n float mGraphLineWidth;\n float mPointSize;\n \n int mFrameIndex;\n int mKeyFrameIndex;\n std::vector keyFrames;\n \n void drawCurrentCamera(pangolin::OpenGlMatrix &Twc);\n void drawCurrentTrajectory(pangolin::OpenGlMatrix &Twc);\n\n void getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &Twc);\n void drawKeyFrames();\n void drawPoints();\n std::vector preTwc;\n };\n}\n\n#include \"KeyFrame.h\"\nnamespace GSLAM{\n \n void Drawer::drawCurrentTrajectory(pangolin::OpenGlMatrix &Twc) {\n \n const float &w = mCameraSize;\n const float h = w*0.75;\n const float z = w*0.6;\n \n glPushMatrix();\n glMultMatrixd(Twc.m);\n \n glLineWidth(mCameraLineWidth);\n glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);\n \n glBegin(GL_LINES);\n glVertex3f(0,0,0);\n glVertex3f(w,h,z);\n glVertex3f(0,0,0);\n glVertex3f(w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,h,z);\n \n glVertex3f(w,h,z);\n glVertex3f(w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(-w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(w,h,z);\n \n glVertex3f(-w,-h,z);\n glVertex3f(w,-h,z);\n glEnd();\n \n glPopMatrix();\n \n preTwc.push_back(Twc);\n \n glPointSize(4);\n glBegin(GL_POINTS);\n for(int i=0;i=keyFrames[mKeyFrameIndex]->mvLocalFrames.back().frameId){\n preTwc.clear();\n mKeyFrameIndex++;\n }\n //printf(\"%d %d\\n\",mFrameIndex,keyFrames[mKeyFrameIndex]->frameId);\n \n Transform pose;\n Transform pose1;\n Transform pose2;\n \n if(mKeyFrameIndex<1){\n \n pose1=keyFrames[mKeyFrameIndex]->pose;\n pose2=keyFrames[mKeyFrameIndex]->pose;\n \n }else{\n\n pose1.rotation=keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.rotation\n *keyFrames[mKeyFrameIndex-1]->pose.rotation;\n \n Eigen::Vector3d translation;\n translation =keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.translation\n /keyFrames[mKeyFrameIndex-1]->scale;\n \n translation =keyFrames[mKeyFrameIndex-1]->pose.rotation.transpose()*translation;\n pose1.translation=keyFrames[mKeyFrameIndex-1]->pose.translation+translation;\n \n pose2=keyFrames[mKeyFrameIndex]->pose;\n }\n \n if (mFrameIndex==keyFrames[mKeyFrameIndex]->frameId) {\n \n pose=pose1;\n \n }else if (mFrameIndexmvLocalFrames[0].frameId) {\n \n double distance1=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId;\n double distance2=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId-mFrameIndex;\n \n Eigen::Vector3d angle1,angle2,angle;\n ceres::RotationMatrixToAngleAxis(pose1.rotation.data(),angle1.data());\n ceres::RotationMatrixToAngleAxis(pose2.rotation.data(),angle2.data());\n angle=(distance2*angle1+distance1*angle2)/(distance1+distance2);\n \n ceres::AngleAxisToRotationMatrix(angle.data(),pose.rotation.data());\n pose.translation=(distance2*pose1.translation+distance1*pose2.translation)/(distance1+distance2);\n \n \n pose1=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].pose;\n ceres::RotationMatrixToAngleAxis(pose1.rotation.data(),angle1.data());\n angle1*=(distance1/(distance1+distance2));\n ceres::AngleAxisToRotationMatrix(angle1.data(),pose1.rotation.data());\n \n pose1.translation=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].pose.translation/keyFrames[mKeyFrameIndex]->scale;\n pose1.translation*=(distance1/(distance1+distance2));\n \n pose.rotation=pose1.rotation*pose.rotation;\n pose.translation+=pose.rotation.transpose()*pose1.translation;\n \n }else{\n \n int localIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId;\n pose.rotation=keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.rotation\n *keyFrames[mKeyFrameIndex]->pose.rotation;\n \n Eigen::Vector3d translation;\n translation =keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.translation\n /keyFrames[mKeyFrameIndex]->scale;\n translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation;\n pose.translation=pose2.translation+translation;\n }\n \n pose.rotation.transposeInPlace();\n \n M.m[0] = pose.rotation(0,0);\n M.m[1] = pose.rotation(1,0);\n M.m[2] = pose.rotation(2,0);\n M.m[3] = 0.0;\n \n M.m[4] = pose.rotation(0,1);\n M.m[5] = pose.rotation(1,1);\n M.m[6] = pose.rotation(2,1);\n M.m[7] = 0.0;\n \n M.m[8] = pose.rotation(0,2);\n M.m[9] = pose.rotation(1,2);\n M.m[10] = pose.rotation(2,2);\n M.m[11] = 0.0;\n \n M.m[12] = pose.translation(0);\n M.m[13] = pose.translation(1);\n M.m[14] = pose.translation(2);\n M.m[15] = 1.0;\n \n mFrameIndex++;\n }\n \n void Drawer::drawPoints(){\n glPointSize(4);\n glBegin(GL_POINTS);\n for(int k=0;kmvLocalMapPoints.size();i++) {\n if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){\n Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition();\n point3D/=keyFrames[k]->scale;\n point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation;\n uchar* color=keyFrames[k]->mvLocalMapPoints[i].color;\n glColor3f(color[2]*(1.0/255.0),color[1]*(1.0/255.0),color[0]*(1.0/255.0));\n glVertex3d(point3D(0),point3D(1),point3D(2));\n }\n }\n }\n glEnd();\n }\n \n void Drawer::drawKeyFrames(){\n \n const float &w = mCameraSize;\n const float h = w*0.75;\n const float z = w*0.6;\n \n std::vector centers(0);\n for(size_t i=0; i<=mKeyFrameIndex; i++){\n \n KeyFrame* pKF = keyFrames[i];\n Transform pose;\n \n if(i==0){\n pose=pKF->pose;\n }else{\n pose.rotation=keyFrames[i-1]->mvLocalFrames.back().pose.rotation\n *keyFrames[i-1]->pose.rotation;\n \n Eigen::Vector3d translation;\n translation =keyFrames[i-1]->mvLocalFrames.back().pose.translation/keyFrames[i-1]->scale;\n \n translation =keyFrames[i-1]->pose.rotation.transpose()*translation;\n pose.translation=keyFrames[i-1]->pose.translation+translation;\n }\n pose.rotation.transposeInPlace();\n centers.push_back(pose.translation);\n \n pangolin::OpenGlMatrix M;\n M.m[0] = pose.rotation(0,0);\n M.m[1] = pose.rotation(1,0);\n M.m[2] = pose.rotation(2,0);\n M.m[3] = 0.0;\n \n M.m[4] = pose.rotation(0,1);\n M.m[5] = pose.rotation(1,1);\n M.m[6] = pose.rotation(2,1);\n M.m[7] = 0.0;\n \n M.m[8] = pose.rotation(0,2);\n M.m[9] = pose.rotation(1,2);\n M.m[10] = pose.rotation(2,2);\n M.m[11] = 0.0;\n \n M.m[12] = pose.translation(0);\n M.m[13] = pose.translation(1);\n M.m[14] = pose.translation(2);\n M.m[15] = 1.0;\n \n glPushMatrix();\n glMultMatrixd(M.m);\n \n glLineWidth(mCameraLineWidth*1.5);\n glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);\n \n glBegin(GL_LINES);\n glVertex3f(0,0,0);\n glVertex3f(w,h,z);\n glVertex3f(0,0,0);\n glVertex3f(w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,h,z);\n \n glVertex3f(w,h,z);\n glVertex3f(w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(-w,-h,z);\n \n glVertex3f(-w,h,z);\n glVertex3f(w,h,z);\n \n glVertex3f(-w,-h,z);\n glVertex3f(w,-h,z);\n glEnd();\n glPopMatrix();\n }\n \n glLineWidth(mGraphLineWidth);\n \n glBegin(GL_LINES);\n //printf(\"show %d keyframes\\n\",centers.size());\n std::vector > lines;\n for(size_t k=0;k<=mKeyFrameIndex;k++)\n {\n // Covisibility Graph\n Eigen::Vector3d center=centers[k];//=keyFrames[k]->pose.translation;\n for(map::iterator mit=keyFrames[k]->mConnectedKeyFramePoses.begin(),\n mend=keyFrames[k]->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n if(mit->first->mnIdmnId){\n if(mit->first->nextKeyFramePtr==keyFrames[k]&&mit->first==keyFrames[k]->prevKeyFramePtr){\n glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);\n Eigen::Vector3d center2=centers[mit->first->mnId];//mit->first->pose.translation;\n glVertex3d(center(0),center(1),center(2));\n glVertex3d(center2(0),center2(1),center2(2));\n \n }else if(mit->second.scale==-1.0){\n Eigen::Vector3d center2=centers[mit->first->mnId];\n if(mit->first->frameId<70||keyFrames[k]->frameId<70){\n lines.push_back(std::make_pair(center,center2));\n continue;\n }\n glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0);\n glVertex3d(center(0),center(1),center(2));\n glVertex3d(center2(0),center2(1),center2(2));\n\n }else{\n glColor3f(237.0f/255.0,177.0f/255.0,32.0f/255.0);\n Eigen::Vector3d center2=centers[mit->first->mnId];//mit->first->pose.translation;\n glVertex3d(center(0),center(1),center(2));\n glVertex3d(center2(0),center2(1),center2(2));\n }\n }\n }\n }\n glEnd();\n \n glLineWidth(mGraphLineWidth);\n glLineStipple(8, 0xAAAA);\n glEnable(GL_LINE_STIPPLE);\n glBegin(GL_LINES);\n \n for(int i=0;i\n#include \"Geometry.h\"\n#include \"RelativeMotion.hpp\"\n#include \"opencv2/calib3d/calib3d.hpp\"\n\n\nbool getPVector(const Eigen::Vector3d &T12,\n const Eigen::Vector3d &p1,\n const Eigen::Vector3d &p2,\n Eigen::Vector3d &pVector){\n \n Eigen::Vector3d T=p1;\n Eigen::Vector3d r1=T12;\n Eigen::Vector3d r2=-p2;\n Eigen::Vector3d xij=r1.cross(r2);\n \n //analytic solution\n Eigen::Matrix3d A;\n for(int i=0;i<3;i++){\n A(i,0)=r1(i);\n A(i,1)=-r2(i);\n A(i,2)=-xij(i);\n }\n double determinantA=A.determinant();\n \n Eigen::Matrix3d D=A;\n D.col(0)=T;\n double determinant1=D.determinant();\n \n A.col(1)=T;\n double determinant2=A.determinant();\n \n double s_ik=determinant1/determinantA;\n double s_jk=determinant2/determinantA;\n \n if(s_ik<0||s_jk<0){\n return false;\n }\n \n Eigen::Matrix3d R_jik=ComputeAxisRotation(T,r1);\n Eigen::Matrix3d R_ijk=ComputeAxisRotation(-T,r2);\n \n Eigen::Matrix3d PMatrix=(Eigen::Matrix3d::Identity()+s_ik*R_jik-s_jk*R_ijk);\n pVector=PMatrix*p1;\n pVector=0.5*pVector;\n return true;\n}\n\n\nvoid getScaleRatio(const Eigen::Vector3d &T12,\n const Eigen::Vector3d &p1,\n const Eigen::Vector3d &p2,\n double &s1,double &s2){\n \n Eigen::Vector3d T=p1;\n Eigen::Vector3d r1=T12;\n Eigen::Vector3d r2=-p2;\n Eigen::Vector3d xij=r1.cross(r2);\n \n //analytic solution\n Eigen::Matrix3d A;\n for(int i=0;i<3;i++){\n A(i,0)=r1(i);\n A(i,1)=-r2(i);\n A(i,2)=-xij(i);\n }\n double determinantA=A.determinant();\n \n Eigen::Matrix3d D=A;\n D.col(0)=T;\n double determinant1=D.determinant();\n \n A.col(1)=T;\n double determinant2=A.determinant();\n \n s1=determinant1/determinantA;\n s2=determinant2/determinantA;\n}\n\n\n\nEigen::Vector3d estimateRelativeTranslationIRLS(const std::vector &pts1,\n const std::vector &pts2,\n std::vector &pVectors,\n std::vector &status){\n int k = 0;\n int kmax = 100;\n int max_in_iter = 10;\n int in_iter = 0;\n double epslon = 1e-5;\n \n \n int num_point=pts1.size();\n Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);\n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n Eigen::Vector3d translationNew=V.col(2);\n \n Eigen::VectorXd weightsNew=(translationNew.transpose()*allNorms);\n weights=weightsNew.cwiseAbs();\n \n double costNew=weights.sum();\n double delt = std::max(std::abs(costVal-costNew),1-translationNew.dot(translation));\n \n if (delt <= epslon){\n in_iter++;\n }\n else{\n in_iter=0;\n }\n \n costVal=costNew;\n translation=translationNew;\n k++;\n break;\n }\n \n pVectors.resize(num_point);\n status.resize(num_point);\n std::fill(status.begin(),status.end(),true);\n \n std::vector s1(num_point),s2(num_point);\n for (int i=0;i0;\n }\n \n bool isPositive=positiveCount>overallCount/2;\n double factor=isPositive?1.0:-1.0;\n translation*=factor;\n \n for (int i=0;i0)==isPositive) {\n \n double s_1=factor*s1[i];\n double s_2=factor*s2[i];\n \n Eigen::Matrix3d R_jik=ComputeAxisRotation(pts1[i],translation);\n Eigen::Matrix3d R_ijk=ComputeAxisRotation(-pts1[i],-pts2[i]);\n \n Eigen::Matrix3d PMatrix=(Eigen::Matrix3d::Identity()+s_1*R_jik-s_2*R_ijk);\n pVectors[i]=0.5*PMatrix*pts1[i];\n }else{\n status[i]=false;\n }\n }\n }\n return translation;\n}\n\n\nEigen::Vector3d estimateRelativeTranslation(const std::vector &pts1,\n const std::vector &pts2,\n std::vector &pVectors,\n std::vector &status){\n \n int num_point=pts1.size();\n std::vector norms(num_point);\n pVectors.resize(num_point);\n status.resize(num_point);\n \n Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);\n Eigen::Vector3d preResult,curResult;\n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n curResult=V.col(2);\n \n int positiveCount=0;\n std::vector flags(num_point);\n for(int i=0;i0);\n positiveCount+=flags[i];\n }\n int flag=1;\n if((double)positiveCount<0.5*(double)num_point){\n curResult*=-1;\n flag=0;\n }\n \n for(int i=0;i &pts1,\n const std::vector &pts2,\n std::vector &status,\n const double inlierPercentage=0.8,\n const double inlierThreshold=std::sin((3.0/180.0)*CV_PI)){\n \n int num_point=pts1.size();\n std::vector residuals(num_point);\n std::vector norms(num_point);\n status.resize(num_point);\n std::vector pVectors(num_point);\n \n Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);\n \n \n \n int numInliers=0;\n double preError=1.0,curError=1.0;\n Eigen::Vector3d preResult,curResult;\n \n for(int iter=0;iter<10;iter++){\n if(iter==0){\n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n curResult=V.col(2);\n Eigen::VectorXd errors=curResult.transpose()*allNorms;\n \n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n curResult=V.col(2);\n Eigen::VectorXd errors=curResult.transpose()*allNorms;\n for(int i=0;i0);\n positiveCount+=residuals[i].flag;\n }\n \n int flag=1;\n if((double)positiveCount<0.5*(double)numInliers){\n preResult*=-1;\n flag=0;\n }\n \n for(int i=0;i &pts1,\n const std::vector &pts2,\n std::vector &pVectors,\n std::vector &status,\n std::vector &errors,\n double &medError,\n const double inlierPercentage=0.8,\n const double inlierThreshold=std::sin((3.0/180.0)*CV_PI)){\n \n int num_point=pts1.size();\n std::vector residuals(num_point);\n std::vector norms(num_point);\n pVectors.resize(num_point);\n status.resize(num_point);\n errors.resize(num_point);\n \n Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);\n \n \n \n int numInliers=0;\n double preError=1.0,curError=1.0;\n Eigen::Vector3d preResult,curResult;\n \n for(int iter=0;iter<10;iter++){\n if(iter==0){\n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n curResult=V.col(2);\n Eigen::VectorXd errors=curResult.transpose()*allNorms;\n \n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n curResult=V.col(2);\n Eigen::VectorXd errors=curResult.transpose()*allNorms;\n for(int i=0;i0);\n positiveCount+=residuals[i].flag;\n }\n \n int flag=1;\n if((double)positiveCount<0.5*(double)numInliers){\n preResult*=-1;\n flag=0;\n }\n \n for(int i=0;i &pts1,\n std::vector &pts2,\n Eigen::Matrix3d &rotation,\n Eigen::Vector3d &translation,\n std::vector &pVectors,\n std::vector &status){\n \n \n estimateRotationTranslation(0.005,\n pts1,\n pts2,\n rotation,\n translation);\n \n //estimateRotationTranslation2(0.005,pts1,pts2,rotation,translation);\n translation=rotation.transpose() * translation;\n translation.normalize();\n \n \n for (int i=0;i flags(num_point);\n for(int i=0;i0);\n positiveCount+=flags[i];\n }\n int flag=1;\n if((double)positiveCount<0.5*(double)num_point){\n translation*=-1;\n flag=0;\n }\n \n status.resize(num_point);\n std::fill(status.begin(),status.end(),false);\n pVectors.resize(num_point);\n \n for(int i=0;i\n bool operator()(const T* const camera,\n const T* const depth,\n T* residuals) const {\n \n T p[3];\n T point[3]={((T)key_x)/depth[0],((T)key_y)/depth[0],1.0/depth[0]};\n ceres::AngleAxisRotatePoint(camera,point,p);\n \n p[0]+=camera[3];\n p[1]+=camera[4];\n p[2]+=camera[5];\n \n T predicted_x=p[0]/p[2];\n T predicted_y=p[1]/p[2];\n \n residuals[0]=predicted_x-(T)tracked_x;\n residuals[1]=predicted_y-(T)tracked_y;\n \n residuals[0]=residuals[0]*weight;\n residuals[1]=residuals[1]*weight;\n \n return true;\n }\n \n static ceres::CostFunction* Create(const double key_x,\n const double key_y,\n const double tracked_x,\n const double tracked_y,\n const double weight=1.0) {\n return (new ceres::AutoDiffCostFunction(new SnavelyReprojectionError(key_x,key_y,tracked_x,tracked_y,weight)));\n };\n \n double tracked_x;\n double tracked_y;\n double key_x;\n double key_y;\n double weight;\n};\n\n\nstruct SnavelyReprojectionErrorSimple {\n \n SnavelyReprojectionErrorSimple(\n const double _key_x,const double _key_y,\n const double _tracked_x,const double _tracked_y):\n key_x(_key_x),key_y(_key_y),\n tracked_x(_tracked_x), tracked_y(_tracked_y){\n }\n \n template \n bool operator()(const T* const camera,\n const T* const depth,\n T* residuals) const {\n \n T ax=key_x-camera[2]*key_y+camera[1];\n T bx=camera[3];\n T ay=key_y-camera[0]+camera[2]*key_x;\n T by=camera[4];\n T c =-camera[1]*key_x+camera[0]*key_y+1.0;\n T d =camera[5];\n T ex=tracked_x*c-ax;\n T ey=tracked_y*c-ay;\n \n if(depth[0]<0.01){\n residuals[0]=ex;\n residuals[1]=ey;\n return true;\n }\n \n T fx=tracked_x*d-bx;\n T fy=tracked_y*d-by;\n \n residuals[0]=(ex+fx*depth[0])/(c+d*depth[0]);\n residuals[1]=(ey+fy*depth[0])/(c+d*depth[0]);\n return true;\n }\n \n static ceres::CostFunction* Create(\n const double key_x,\n const double key_y,\n const double tracked_x,\n const double tracked_y) {\n return (new ceres::AutoDiffCostFunction(new SnavelyReprojectionErrorSimple(key_x,key_y,tracked_x,tracked_y)));\n }\n \n double tracked_x;\n double tracked_y;\n double key_x;\n double key_y;\n};\n\nvoid setSmallRotationMatrix(const double *w,Eigen::Matrix3d &rotation){\n rotation(0,0)=rotation(1,1)=rotation(2,2)=1.0;\n rotation(0,1)=-w[2];rotation(0,2)=w[1];\n rotation(1,0)=w[2];rotation(1,2)=-w[0];\n rotation(2,0)=-w[1];rotation(2,1)=w[0];\n}\n\nnamespace GSLAM{\n \n int RelativeOutlierRejection::relativeRansac(const KeyFrame *keyFrame,const KLT_FeatureList featureList){\n \n assert(keyFrame->mvLocalMapPoints.size()==featureList->nFeatures);\n \n vector points1,points2;\n vector indices;\n points1.reserve(featureList->nFeatures);\n points2.reserve(featureList->nFeatures);\n indices.reserve(featureList->nFeatures);\n \n int medViewAngleCount=0,overallCount=0;\n Eigen::Matrix3d rotationTranspoed=rotation.transpose();\n \n \n for (int i=0;inFeatures;i++) {\n if(featureList->feature[i]->val==KLT_TRACKED&&(*isValid)[i]){\n \n overallCount++;\n \n Eigen::Vector3d p1=keyFrame->mvLocalMapPoints[i].norm;\n Eigen::Vector3d p2=rotationTranspoed*featureList->feature[i]->norm;\n p2.normalize();\n double viewAngle=p1.dot(p2);\n \n if (viewAngle>=minViewAngle) {\n (*isValid)[i]=false;\n continue;\n }\n \n if (viewAngle status;\n cv::findFundamentalMat(points1,points2,cv::FM_RANSAC,theshold,prob,status);\n \n int inlierCount=status.size();\n for (int i=0;i &pVectors){\n \n \n std::vector viewAngles(keyFrame->mvLocalMapPoints.size());\n std::fill(viewAngles.begin(),viewAngles.end(),-1.0);\n \n pVectors.resize(keyFrame->mvLocalMapPoints.size());\n std::fill(pVectors.begin(),pVectors.end(),static_cast(NULL));\n \n std::vector norms1,norms2;\n std::vector indices;\n \n indices.reserve(keyFrame->mvLocalMapPoints.size());\n norms1.reserve(keyFrame->mvLocalMapPoints.size());\n norms2.reserve(keyFrame->mvLocalMapPoints.size());\n \n \n \n for (int i=0;imvLocalMapPoints.size();i++) {\n if (featureList->feature[i]->val==KLT_TRACKED&&(*isValid)[i]) {\n Eigen::Vector3d norm2=rotation.transpose()*featureList->feature[i]->norm;\n norm2.normalize();\n norms1.push_back(keyFrame->mvLocalMapPoints[i].norm);\n norms2.push_back(norm2);\n indices.push_back(i);\n }\n }\n norms1.shrink_to_fit();\n norms2.shrink_to_fit();\n indices.shrink_to_fit();\n \n std::vector status;\n std::vector tmpPVectors;\n translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status);\n \n //translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status);\n //Eigen::Matrix3d inputRotation;\n //Eigen::Vector3d inputTranslation;\n \n //this->rotation=Eigen::Matrix3d::Identity();\n //this->translation=Eigen::Vector3d::Zero();\n \n //estimateRelativeMotion(norms1,norms2,this->rotation,this->translation,tmpPVectors,status);\n \n /*Eigen::Matrix3d transposedRotation=rotation.transpose();\n for (int i=0;i indices(0);\n for(int i=0;imvLocalMapPoints.size();i++){\n keyFrame->mvLocalMapPoints[i].isFullTrack&=(!keyFrame->mvLocalMapPoints[i].vecs.empty());\n if(keyFrame->mvLocalMapPoints[i].isFullTrack){\n indices.push_back(i);\n }\n }\n \n int num_frame=keyFrame->mvLocalFrames.size();\n int num_point=indices.size();\n \n //printf(\"%d %d\\n\",num_frame,num_point);getchar();\n \n DMat denseMatrix=svdNewDMat(num_point,3*num_frame);\n for(int p=0;p& pVectors=keyFrame->mvLocalMapPoints[ind].pVectors;\n for(int f=0;fvalue[p][3*f],&(*pVectors[f])(0),3*sizeof(double));\n }\n }\n SMat sparseMatrix=svdConvertDtoS(denseMatrix);\n SVDRec svdResult=svdLAS2A(sparseMatrix,12);\n \n#ifdef DEBUG\n printf(\"singular %f %f\\n\",svdResult->S[0],svdResult->S[1]);\n#endif\n \n \n svdFreeDMat(denseMatrix);\n svdFreeSMat(sparseMatrix);\n \n \n int positiveCount=0;\n for(int p=0;pUt->value[0][p]>0);\n }\n \n if(positiveCountUt->value[0][p]*=positiveCount;\n int ind=indices[p];\n if (svdResult->Ut->value[0][p]>0) {\n keyFrame->mvLocalMapPoints[ind].invdepth=svdResult->Ut->value[0][p]/keyFrame->mvLocalMapPoints[ind].norm(2);\n keyFrame->mvLocalMapPoints[ind].isEstimated=true;\n }\n }\n \n for(int f=0;fmvLocalFrames[f].pose.translation=Eigen::Vector3d(&svdResult->Vt->value[0][3*f]);\n keyFrame->mvLocalFrames[f].pose.translation*=positiveCount*svdResult->S[0];\n }\n }\n \n void LocalFactorization::iterativeRefine(KeyFrame *keyFrame){\n \n std::vector indices(0);\n for(int i=0;imvLocalMapPoints.size();i++){\n keyFrame->mvLocalMapPoints[i].isFullTrack&=(!keyFrame->mvLocalMapPoints[i].vecs.empty());\n if(keyFrame->mvLocalMapPoints[i].isFullTrack){\n indices.push_back(i);\n }\n }\n std::vector vecs1(indices.size());\n std::vector vecs2(indices.size());\n \n /*for (int i=0;imvLocalMapPoints[indices[i]].vec;\n vecs0[i].x=vec(0);\n vecs0[i].y=vec(1);\n }*/\n \n \n std::ofstream of(\"/Users/ctang/Desktop/debug/res.txt\");\n //double converge=0.002;\n for (int f=keyFrame->mvLocalFrames.size()-5;fmvLocalFrames.size()-4;f++) {\n for (int i=0;imvLocalMapPoints[indices[i]].getPosition();\n Eigen::Vector3d vec1=keyFrame->mvLocalFrames[f].pose.rotation*(point3d-keyFrame->mvLocalFrames[f].pose.translation);\n vec1/=vec1(2);\n Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[indices[i]].vecs[f];\n vecs1[i].x=vec1(0);\n vecs1[i].y=vec1(1);\n \n vecs2[i].x=vec2(0);\n vecs2[i].y=vec2(1);\n }\n std::vector statuts;\n cv::Mat homography=cv::findHomography(vecs1,vecs2,CV_RANSAC,0.005,statuts);\n std::vector rotation(1),translation(1),normal(1);\n cv::Mat calib=cv::Mat::eye(3,3,CV_64FC1);\n cv::decomposeHomographyMat(homography,calib,\n rotation,translation,normal);\n /*printf(\"%d\\n\",f);\n for (int i1=0;i1<3;i1++) {\n for (int i2=0;i2<3;i2++) {\n printf(\"%f \",rotation[0].at(i1,i2));\n }\n printf(\"\\n\");\n }*/\n //printf(\"%d\\n\",rotation.size());\n //getchar();\n for (int i=0;i(0)=vecs1[i].x;\n vec1.at(1)=vecs1[i].y;\n vec1.at(2)=1.0;\n \n double diffx=vec1.at(0)-vecs2[i].x;\n double diffy=vec1.at(1)-vecs2[i].y;\n \n of<(2);\n diffx=_vec1.at(0)-vecs2[i].x;\n diffy=_vec1.at(1)-vecs2[i].y;\n of< indices;\n \n for (int p=0;pmvLocalMapPoints.size();p++) {\n if (keyFrame->mvLocalMapPoints[p].isEstimated) {\n indices.push_back(p);\n num_point++;\n }\n }\n \n int num_frame=keyFrame->mvLocalFrames.size();\n std::vector pointNumberInFrames(num_frame,num_point);\n \n std::vector param_cam(6*num_frame),param_invDepth(num_point);\n ceres::Problem problem;\n printf(\"%d %d\\n\",num_point,num_frame);\n \n for(int p=0;pmvLocalMapPoints[index].invdepth;\n }\n \n for(int f=0;fmvLocalFrames[f].pose.rotation(0),¶m_cam[6*f]);\n Eigen::Vector3d translation=keyFrame->mvLocalFrames[f].pose.rotation*keyFrame->mvLocalFrames[f].pose.translation;\n param_cam[6*f+3]=-translation(0);\n param_cam[6*f+4]=-translation(1);\n param_cam[6*f+5]=-translation(2);\n }\n \n ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005);\n \n for(int p=0;pmvLocalMapPoints[index].vec;\n for(int f=0;fmvLocalMapPoints[index].vecs[f]==NULL){\n continue;\n }\n \n Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[index].vecs[f];\n ceres::CostFunction* cost_function =SnavelyReprojectionError::Create(vec1(0),vec1(1),vec2(0),vec2(1));\n problem.AddResidualBlock(cost_function,loss_function,¶m_cam[6*f],¶m_invDepth[p]);\n }\n }\n if (cameraFixed) {\n for(int f=0;fmvLocalFrames[f].pose.rotation;\n ceres::AngleAxisToRotationMatrix(¶m_cam[6*f],&keyFrame->mvLocalFrames[f].pose.rotation(0));\n //rotation=rotation.transpose()*keyFrame->mvLocalFrames[f].pose.rotation;\n //std::cout<mvLocalFrames[f].pose.translation=-Eigen::Vector3d(¶m_cam[6*f+3]);\n keyFrame->mvLocalFrames[f].pose.translation=keyFrame->mvLocalFrames[f].pose.rotation.transpose()\n *keyFrame->mvLocalFrames[f].pose.translation;\n keyFrame->mvLocalFrames[f].pose.setEssentialMatrix();\n }\n \n for(int p=0;pmvLocalMapPoints[index].invdepth=param_invDepth[p];\n keyFrame->mvLocalMapPoints[index].updateNormalAndDepth();\n }\n \n for(int p=0;pmvLocalMapPoints[index].getPosition();\n \n if(keyFrame->mvLocalMapPoints[index].errors.empty()){\n keyFrame->mvLocalMapPoints[index].errors.resize(num_frame);\n }\n \n for (int f=0;fmvLocalMapPoints[index].vecs[f]==NULL){\n continue;\n }\n \n Eigen::Vector3d projected=keyFrame->mvLocalFrames[f].pose.rotation\n *(point3D-keyFrame->mvLocalFrames[f].pose.translation);\n projected/=projected(2);\n projected-=(*keyFrame->mvLocalMapPoints[index].vecs[f]);\n double error=projected.norm();\n keyFrame->mvLocalMapPoints[index].errors[f]=error;\n \n if (error>projErrorThres) {\n delete keyFrame->mvLocalMapPoints[index].vecs[f];\n keyFrame->mvLocalMapPoints[index].vecs[f]=NULL;\n keyFrame->mvLocalMapPoints[index].measurementCount--;\n keyFrame->mvLocalFrames[f].measurementCount--;\n }\n }\n \n //assert(keyFrame->mvLocalMapPoints[index].measurementCount>0);\n if (keyFrame->mvLocalMapPoints[index].measurementCount==0||keyFrame->mvLocalMapPoints[index].invdepth<0) {\n keyFrame->mvLocalMapPoints[index].isEstimated=false;\n }\n }\n \n /*for (int i=0;imvLocalMapPoints.size();i++) {\n \n }*/\n \n int measurementCountByPoint=0,measurementCountByFrame=0;\n for (int p=0;pmvLocalMapPoints.size();p++) {\n measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;\n }\n \n for (int f=0;fmvLocalFrames.size();f++) {\n measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;\n }\n printf(\"%d %d\\n\",measurementCountByPoint,measurementCountByFrame);\n assert(measurementCountByPoint==measurementCountByFrame);\n }\n \n \n \n void LocalBundleAdjustment::triangulate(KeyFrame* keyFrame){\n \n int num_frame=keyFrame->mvLocalFrames.size();\n \n for (int p=0;pmvLocalMapPoints.size();p++) {\n \n if(!keyFrame->mvLocalMapPoints[p].isEstimated&&keyFrame->mvLocalMapPoints[p].isValid()){\n \n std::vector normalized;\n std::vector rotations;\n std::vector cameras;\n \n normalized.reserve(num_frame);\n rotations.reserve(num_frame);\n cameras.reserve(num_frame);\n \n normalized.push_back(keyFrame->mvLocalMapPoints[p].norm);\n rotations.push_back(Eigen::Matrix3d::Identity());\n cameras.push_back(Eigen::Vector3d::Zero());\n \n for (int f=0;fmvLocalMapPoints[p].vecs[f]==NULL) {\n continue;\n }\n Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[p].vecs[f];\n Eigen::Vector3d norm2=keyFrame->mvLocalFrames[f].pose.rotation.transpose()*vec2;\n norm2.normalize();\n \n double viewAngle=keyFrame->mvLocalMapPoints[p].norm.dot(norm2);\n \n if (viewAngle>viewAngleThres) {\n \n delete keyFrame->mvLocalMapPoints[p].vecs[f];\n keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;\n \n keyFrame->mvLocalMapPoints[p].measurementCount--;\n keyFrame->mvLocalFrames[f].measurementCount--;\n \n }else{\n \n Eigen::Vector3d epiolarLine=keyFrame->mvLocalFrames[f].pose.E*keyFrame->mvLocalMapPoints[p].vec;\n double squareError=epiolarLine.dot(vec2);\n squareError*=squareError;\n squareError/=(epiolarLine(0)*epiolarLine(0)+epiolarLine(1)*epiolarLine(1));\n \n if (squareError>4*projErrorThres*projErrorThres) {\n \n delete keyFrame->mvLocalMapPoints[p].vecs[f];\n keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;\n \n keyFrame->mvLocalMapPoints[p].measurementCount--;\n keyFrame->mvLocalFrames[f].measurementCount--;\n \n }else{\n normalized.push_back(norm2);\n rotations.push_back(Eigen::Matrix3d::Identity());\n cameras.push_back(keyFrame->mvLocalFrames[f].pose.translation);\n }\n }\n }\n \n normalized.shrink_to_fit();\n rotations.shrink_to_fit();\n cameras.shrink_to_fit();\n \n if (cameras.size()>1) {\n \n Eigen::Vector3d point3D=multiviewTriangulationLinear(normalized,rotations,cameras);\n \n bool isOutlier=false;\n for(int i=1;i2*projErrorThres) {\n isOutlier=true;\n break;\n }\n }\n \n if (point3D(2)<0) {\n isOutlier=true;\n }\n \n if (!isOutlier) {\n keyFrame->mvLocalMapPoints[p].isEstimated=true;\n keyFrame->mvLocalMapPoints[p].invdepth=1.0/point3D(2);\n }else{\n for (int f=0;fmvLocalMapPoints[p].vecs[f]!=NULL) {\n delete keyFrame->mvLocalMapPoints[p].vecs[f];\n keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;\n keyFrame->mvLocalFrames[f].measurementCount--;\n }\n }\n keyFrame->mvLocalMapPoints[p].measurementCount=0;\n }\n }\n }\n }\n //zheli\n \n int measurementCountByPoint=0,measurementCountByFrame=0;\n for (int p=0;pmvLocalMapPoints.size();p++) {\n measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;\n }\n \n for (int f=0;fmvLocalFrames.size();f++) {\n measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;\n }\n assert(measurementCountByPoint==measurementCountByFrame);\n \n int effectiveCount=0;\n for(int i=0;imvLocalMapPoints.size();i++){\n effectiveCount+=keyFrame->mvLocalMapPoints[i].isValid()==true;\n }\n printf(\"%d\\n\",effectiveCount);\n }\n \n \n \n //refine points by projection error\n void LocalBundleAdjustment::refinePoints(KeyFrame *keyFrame){\n \n int num_point=0;\n std::vector indices;\n \n for (int p=0;pmvLocalMapPoints.size();p++) {\n if (keyFrame->mvLocalMapPoints[p].isEstimated) {\n indices.push_back(p);\n num_point++;\n }\n }\n int num_frame=keyFrame->mvLocalFrames.size();\n \n \n for(int p=0;pmvLocalMapPoints[index].getPosition();\n \n if(keyFrame->mvLocalMapPoints[index].errors.empty()){\n keyFrame->mvLocalMapPoints[index].errors.resize(num_frame);\n }\n \n for (int f=0;fmvLocalMapPoints[index].vecs[f]==NULL){\n continue;\n }\n \n Eigen::Vector3d projected=keyFrame->mvLocalFrames[f].pose.rotation\n *(point3D-keyFrame->mvLocalFrames[f].pose.translation);\n projected/=projected(2);\n projected-=(*keyFrame->mvLocalMapPoints[index].vecs[f]);\n double error=projected.norm();\n keyFrame->mvLocalMapPoints[index].errors[f]=error;\n \n if (error>projErrorThres) {\n delete keyFrame->mvLocalMapPoints[index].vecs[f];\n keyFrame->mvLocalMapPoints[index].vecs[f]=NULL;\n keyFrame->mvLocalMapPoints[index].measurementCount--;\n keyFrame->mvLocalFrames[f].measurementCount--;\n }\n }\n \n if (keyFrame->mvLocalMapPoints[index].measurementCount==0\n ||keyFrame->mvLocalMapPoints[index].invdepth<0) {\n keyFrame->mvLocalMapPoints[index].isEstimated=false;\n }\n }\n \n \n int measurementCountByPoint=0,measurementCountByFrame=0;\n for (int p=0;pmvLocalMapPoints.size();p++) {\n measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;\n }\n \n for (int f=0;fmvLocalFrames.size();f++) {\n measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;\n }\n printf(\"%d %d\\n\",measurementCountByPoint,measurementCountByFrame);\n assert(measurementCountByPoint==measurementCountByFrame);\n }\n \n \n \n void LocalBundleAdjustment::triangulate2(KeyFrame* keyFrame){\n \n int num_frame=keyFrame->mvLocalFrames.size();\n \n for (int p=0;pmvLocalMapPoints.size();p++) {\n \n if(!keyFrame->mvLocalMapPoints[p].isEstimated&&keyFrame->mvLocalMapPoints[p].isValid()){\n \n std::vector normalized;\n std::vector rotations;\n std::vector cameras;\n \n normalized.reserve(num_frame);\n rotations.reserve(num_frame);\n cameras.reserve(num_frame);\n \n normalized.push_back(keyFrame->mvLocalMapPoints[p].norm);\n rotations.push_back(Eigen::Matrix3d::Identity());\n cameras.push_back(Eigen::Vector3d::Zero());\n \n for (int f=0;fmvLocalMapPoints[p].vecs[f]==NULL) {\n continue;\n }\n Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[p].vecs[f];\n Eigen::Vector3d norm2=keyFrame->mvLocalFrames[f].pose.rotation.transpose()*vec2;\n norm2.normalize();\n \n double viewAngle=keyFrame->mvLocalMapPoints[p].norm.dot(norm2);\n \n if (viewAngle>viewAngleThres) {\n \n delete keyFrame->mvLocalMapPoints[p].vecs[f];\n keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;\n \n keyFrame->mvLocalMapPoints[p].measurementCount--;\n keyFrame->mvLocalFrames[f].measurementCount--;\n \n }else{\n \n Eigen::Vector3d epiolarLine=keyFrame->mvLocalFrames[f].pose.E\n *keyFrame->mvLocalMapPoints[p].vec;\n \n double squareError=epiolarLine.dot(vec2);\n squareError*=squareError;\n squareError/=(epiolarLine(0)*epiolarLine(0)+epiolarLine(1)*epiolarLine(1));\n \n if (squareError>4*projErrorThres*projErrorThres) {\n \n delete keyFrame->mvLocalMapPoints[p].vecs[f];\n keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;\n \n keyFrame->mvLocalMapPoints[p].measurementCount--;\n keyFrame->mvLocalFrames[f].measurementCount--;\n \n }else{\n normalized.push_back(norm2);\n rotations.push_back(Eigen::Matrix3d::Identity());\n cameras.push_back(keyFrame->mvLocalFrames[f].pose.translation);\n }\n }\n }\n \n normalized.shrink_to_fit();\n rotations.shrink_to_fit();\n cameras.shrink_to_fit();\n \n if (cameras.size()>1) {\n \n Eigen::Vector3d point3D=multiviewTriangulationLinear(normalized,rotations,cameras);\n //double depth=multiviewTriangulationDepth(normalized,rotations,cameras);\n //std::cout<2*projErrorThres) {\n isOutlier=true;\n break;\n }\n }\n \n if (point3D(2)<0) {\n isOutlier=true;\n }\n \n if (!isOutlier) {\n keyFrame->mvLocalMapPoints[p].isEstimated=true;\n keyFrame->mvLocalMapPoints[p].invdepth=1.0/point3D(2);\n }else{\n for (int f=0;fmvLocalMapPoints[p].vecs[f]!=NULL) {\n delete keyFrame->mvLocalMapPoints[p].vecs[f];\n keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;\n keyFrame->mvLocalFrames[f].measurementCount--;\n }\n }\n keyFrame->mvLocalMapPoints[p].measurementCount=0;\n }\n }\n }\n }\n \n int measurementCountByPoint=0,measurementCountByFrame=0;\n for (int p=0;pmvLocalMapPoints.size();p++) {\n measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;\n }\n \n for (int f=0;fmvLocalFrames.size();f++) {\n measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;\n }\n assert(measurementCountByPoint==measurementCountByFrame);\n \n int effectiveCount=0;\n for(int i=0;imvLocalMapPoints.size();i++){\n effectiveCount+=keyFrame->mvLocalMapPoints[i].isValid()==true;\n }\n printf(\"%d\\n\",effectiveCount);\n }\n\n \n /*void LocalBundleAdjustment::estimateFarFrames(KeyFrame* keyFrame){\n \n for (int i=0;imvFarFrames.size();i++) {\n \n std::vector& closeMeasurements=keyFrame->mvFarMeasurements[i];\n \n std::vector points3d;\n std::vector points2d;\n \n for (int m=0;mmvLocalMapPoints[m].measurementCount>0) {\n \n cv::Point3f point3d;\n cv::Point2f point2d;\n Eigen::Vector3d point3D=keyFrame->mvLocalMapPoints[m].getPosition();\n \n point3d.x=point3D(0);\n point3d.y=point3D(1);\n point3d.z=point3D(2);\n \n Eigen::Vector3d normalized3d=*closeMeasurements[m];\n point2d.x=normalized3d(0)/normalized3d(2);\n point2d.y=normalized3d(1)/normalized3d(2);\n \n points3d.push_back(point3d);\n points2d.push_back(point2d);\n \n //printf(\"3d %f %f %f %f %f\\n\",point3d.x,point3d.y,point3d.z,point2d.x,point2d.y);\n }\n }\n assert(points3d.size()>10);\n \n cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1);\n cv::Mat rVec,tVec,rMat;\n \n cv::solvePnPRansac(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec,false,1000,0.008,0.99);\n cv::Rodrigues(rVec,rMat);\n \n Transform pose;\n for(int r1=0;r1<3;r1++){\n for(int r2=0;r2<3;r2++){\n pose.rotation(r1,r2)=rMat.at(r1,r2);\n }\n }\n \n tVec=-rMat.t()*tVec;\n assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1);\n pose.translation(0)=tVec.at(0);\n pose.translation(1)=tVec.at(1);\n pose.translation(2)=tVec.at(2);\n \n \n double param_cam[6];\n ceres::RotationMatrixToAngleAxis(&pose.rotation(0),¶m_cam[0]);\n Eigen::Vector3d translation=pose.rotation*pose.translation;\n param_cam[3]=-translation(0);\n param_cam[4]=-translation(1);\n param_cam[5]=-translation(2);\n \n vector param_invDepth(points3d.size());\n ceres::Problem problem;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005);\n int nInliers=0;\n for (int i=0;i(0)=points3d[i].x;\n point3dMat.at(1)=points3d[i].y;\n point3dMat.at(2)=points3d[i].z;\n \n \n cv::Mat proj=rMat*point3dMat+tVec;\n proj/=proj.at(2);\n \n double diffx=proj.at(0)-points2d[i].x;\n double diffy=proj.at(1)-points2d[i].y;\n double error=std::sqrt(diffx*diffx+diffy*diffy);\n \n param_invDepth[i]=1.0/point3dMat.at(2);\n point3dMat/=point3dMat.at(2);\n \n if (error<0.005) {\n ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(point3dMat.at(0),\n point3dMat.at(1),\n (double)points2d[i].x,\n (double)points2d[i].y);\n problem.AddResidualBlock(cost_function,loss_function,param_cam,¶m_invDepth[i]);\n problem.SetParameterBlockConstant(¶m_invDepth[i]);\n nInliers++;\n }\n }\n \n ceres::Solver::Options options;\n options.max_num_iterations=5;\n options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;\n options.minimizer_progress_to_stdout=false;\n ceres::Solver::Summary summary;\n ceres::Solve(options,&problem, &summary);\n \n \n ceres::AngleAxisToRotationMatrix(param_cam,&(keyFrame->mvFarFrames[i].pose.rotation(0)));\n keyFrame->mvFarFrames[i].pose.translation=-Eigen::Vector3d(¶m_cam[3]);\n keyFrame->mvFarFrames[i].pose.translation=keyFrame->mvFarFrames[i].pose.rotation.transpose()\n *keyFrame->mvFarFrames[i].pose.translation;\n keyFrame->mvFarFrames[i].pose.setEssentialMatrix();\n }\n return;\n }*/\n \n \n void LocalBundleAdjustment::refineKeyFrameConnection(KeyFrame* keyFrame){\n \n return;\n int num_keyframe=keyFrame->mConnectedKeyFrameMatches.size();\n int num_point=keyFrame->mvLocalMapPoints.size();\n \n if (num_keyframe==0||(keyFrame->nextKeyFramePtr!=NULL&&num_keyframe==1)) {\n return;\n }\n printf(\"%d %d\\n\",num_keyframe,num_point);\n std::vector param_cam(6*num_keyframe);\n std::vector param_invDepth(num_point);\n std::map keyFrameIndex;\n \n int f=0;\n for(map::iterator mit=keyFrame->mConnectedKeyFramePoses.begin(),\n mend=keyFrame->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n if (mit->first==keyFrame->nextKeyFramePtr) {//next keyframe's pose is by bundle\n continue;\n }\n \n keyFrameIndex[mit->first]=f;\n ceres::RotationMatrixToAngleAxis(&(mit->second.rotation(0)),¶m_cam[6*f]);\n Eigen::Vector3d translation=mit->second.rotation*mit->second.translation;\n param_cam[6*f+3]=-translation(0);\n param_cam[6*f+4]=-translation(1);\n param_cam[6*f+5]=-translation(2);\n f++;\n }\n \n std::vector depthFixed(num_point,true);\n \n for (int p=0;pmvLocalMapPoints.size();p++) {\n param_invDepth[p]=keyFrame->mvLocalMapPoints[p].invdepth;\n }\n \n ceres::Problem problem;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005);\n \n for(map >::iterator mit=keyFrame->mConnectedKeyFrameMatches.begin(),\n mend=keyFrame->mConnectedKeyFrameMatches.end();\n mit!=mend;\n mit++){\n \n KeyFrame* connectedKeyFrame=mit->first;\n if (connectedKeyFrame==keyFrame->nextKeyFramePtr) {\n continue;\n }\n \n f=keyFrameIndex[connectedKeyFrame];\n std::vector &matches=mit->second;\n for (int i=0;imvLocalMapPoints[i].isEstimated) {\n assert(0);\n std::vector points(2);\n std::vector rotations(2,Eigen::Matrix3d::Identity());\n std::vector cameras(2);\n \n Transform transform=keyFrame->mConnectedKeyFramePoses[connectedKeyFrame];\n \n points[0]=keyFrame->mvLocalMapPoints[i].norm;\n points[1]=keyFrame->mvLocalMapPoints[matches[i]].norm;\n points[1]=transform.rotation.transpose()*points[1];\n \n cameras[0]=Eigen::Vector3d::Zero();\n cameras[1]=transform.translation;\n \n //assert(keyFrame->mvLocalMapPoints[i].previousInvDepth==0.0);\n Eigen::Vector3d point3D=multiviewTriangulationLinear(points,rotations,cameras);\n keyFrame->mvLocalMapPoints[i].invdepth=1.0/point3D(2);\n keyFrame->mvLocalMapPoints[i].isEstimated=true;\n \n }\n assert(keyFrame->mvLocalMapPoints[i].isEstimated);\n \n Eigen::Vector3d keyFramePoint=keyFrame->mvLocalMapPoints[i].vec;\n Eigen::Vector3d framePoint=connectedKeyFrame->mvLocalMapPoints[matches[i]].vec;\n ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(keyFramePoint(0),keyFramePoint(1),\n framePoint(0),framePoint(1));\n problem.AddResidualBlock(cost_function,loss_function,¶m_cam[6*f],¶m_invDepth[i]);\n }\n }\n \n for (int i=0;imvLocalMapPoints[i].isEstimated&&keyFrame->mvLocalMapPoints[i].measurementCount>0) {\n problem.SetParameterBlockConstant(¶m_invDepth[i]);\n }\n }\n \n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_SCHUR;\n options.minimizer_progress_to_stdout=false;\n options.logging_type=ceres::SILENT;\n ceres::Solver::Summary summary;\n ceres::Solve(options,&problem, &summary);\n \n \n for (int i=0;imvLocalMapPoints[i].isEstimated&&keyFrame->mvLocalMapPoints[i].measurementCount<=0) {\n keyFrame->mvLocalMapPoints[i].invdepth=param_invDepth[i];\n keyFrame->mvLocalMapPoints[i].updateNormalAndDepth();\n }\n }\n for(map::iterator mit=keyFrame->mConnectedKeyFramePoses.begin(),\n mend=keyFrame->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n if (mit->first==keyFrame->nextKeyFramePtr) {\n continue;\n }\n f=keyFrameIndex[mit->first];\n ceres::AngleAxisToRotationMatrix(¶m_cam[6*f],&(mit->second.rotation(0)));\n mit->second.translation=-Eigen::Vector3d(¶m_cam[6*f+3]);\n mit->second.translation=mit->second.rotation.transpose()*mit->second.translation;\n mit->second.setEssentialMatrix();\n }\n return;\n }\n \n int LocalBundleAdjustment::refineKeyFrameMatches(KeyFrame *keyFrame1,const KeyFrame *keyFrame2,Transform &transform,\n vector& matches12,vector& matches21){\n int nMatches=0;\n ceres::Problem problem;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(projErrorThres);\n \n double param_cam[6];\n \n ceres::RotationMatrixToAngleAxis(&(transform.rotation(0)),¶m_cam[0]);\n Eigen::Vector3d translation=transform.rotation*transform.translation;\n param_cam[3]=-translation(0);\n param_cam[4]=-translation(1);\n param_cam[5]=-translation(2);\n \n vector param_invDepth(matches12.size());\n std::vector depthFixed(matches12.size(),false);\n \n for (int i=0;imvLocalMapPoints[i].isEstimated) {\n std::vector points(2);\n std::vector rotations(2,Eigen::Matrix3d::Identity());\n std::vector cameras(2);\n \n points[0]=keyFrame1->mvLocalMapPoints[i].norm;\n points[1]=keyFrame2->mvLocalMapPoints[matches12[i]].norm;\n points[1]=transform.rotation.transpose()*points[1];\n \n cameras[0]=Eigen::Vector3d::Zero();\n cameras[1]=transform.translation;\n \n Eigen::Vector3d point3D=multiviewTriangulationLinear(points,rotations,cameras);\n keyFrame1->mvLocalMapPoints[i].invdepth=1.0/point3D(2);\n }else{\n depthFixed[i]=true;\n }\n param_invDepth[i]=keyFrame1->mvLocalMapPoints[i].invdepth;\n }\n \n for (int i=0;imvLocalMapPoints[i].vec;\n Eigen::Vector3d framePoint=keyFrame2->mvLocalMapPoints[matches12[i]].vec;\n ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(keyFramePoint(0),keyFramePoint(1),\n framePoint(0),framePoint(1));\n problem.AddResidualBlock(cost_function,loss_function,param_cam,¶m_invDepth[i]);\n }\n }\n \n for (int i=0;iframeId,keyFrame2->frameId,nMatches);\n for (int i=0;imvLocalMapPoints[i].getPosition();\n Eigen::Vector3d proj=transform.rotation*(point3D-transform.translation);\n proj/=proj(2);\n double error=(proj-keyFrame2->mvLocalMapPoints[matches12[i]].vec).norm();\n \n if (error>projErrorThres||param_invDepth[i]<0) {\n if(!matches21.empty()){\n matches21[matches12[i]]=-1;\n }\n matches12[i]=-1;\n nMatches--;\n }else if(!depthFixed[i]){\n keyFrame1->mvLocalMapPoints[i].isEstimated=true;\n keyFrame1->mvLocalMapPoints[i].invdepth=param_invDepth[i];\n keyFrame1->mvLocalMapPoints[i].updateNormalAndDepth();\n }\n }\n \n ceres::AngleAxisToRotationMatrix(param_cam,&(transform.rotation(0)));\n transform.translation=-Eigen::Vector3d(¶m_cam[3]);\n transform.translation=transform.rotation.transpose()*transform.translation;\n transform.setEssentialMatrix();\n return nMatches;\n }\n \n \n \n int PnPEstimator::estimate(KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n const vector& matches,Transform& pose){\n \n \n std::vector indices;\n std::vector points3d;\n std::vector points2d;\n \n indices.reserve(matches.size());\n points3d.reserve(matches.size());\n points2d.reserve(matches.size());\n \n //std::cout<<\"size \"<20);\n for(int i=0;i=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){\n \n cv::Point3f point3d;\n cv::Point2f point2d;\n Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();\n \n point3d.x=point3D(0);\n point3d.y=point3D(1);\n point3d.z=point3D(2);\n \n Eigen::Vector3d normalized3d=keyFrame2->mvLocalMapPoints[matches[i]].norm;\n point2d.x=normalized3d(0)/normalized3d(2);\n point2d.y=normalized3d(1)/normalized3d(2);\n \n points3d.push_back(point3d);\n points2d.push_back(point2d);\n indices.push_back(i);\n }\n }\n \n points3d.shrink_to_fit();\n points2d.shrink_to_fit();\n indices.shrink_to_fit();\n int nInliers=indices.size();\n \n cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1);\n cv::Mat rVec,tVec,rMat;\n \n cv::solvePnP(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec);\n cv::Rodrigues(rVec,rMat);\n \n for(int r1=0;r1<3;r1++){\n for(int r2=0;r2<3;r2++){\n pose.rotation(r1,r2)=rMat.at(r1,r2);\n }\n }\n \n tVec=-rMat.t()*tVec;\n assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1);\n pose.translation(0)=tVec.at(0);\n pose.translation(1)=tVec.at(1);\n pose.translation(2)=tVec.at(2);\n \n \n return nInliers;\n }\n \n int PnPEstimator::estimate(KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n vector& matches12,vector& matches21,Transform& pose){\n \n \n std::vector indices;\n std::vector points3d;\n std::vector points2d;\n \n indices.reserve(matches12.size());\n points3d.reserve(matches12.size());\n points2d.reserve(matches12.size());\n \n //std::cout<<\"size \"<=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){\n \n cv::Point3f point3d;\n cv::Point2f point2d;\n Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();\n \n point3d.x=point3D(0);\n point3d.y=point3D(1);\n point3d.z=point3D(2);\n \n Eigen::Vector3d normalized3d=keyFrame2->mvLocalMapPoints[matches12[i]].norm;\n point2d.x=normalized3d(0)/normalized3d(2);\n point2d.y=normalized3d(1)/normalized3d(2);\n \n points3d.push_back(point3d);\n points2d.push_back(point2d);\n indices.push_back(i);\n }\n }\n \n points3d.shrink_to_fit();\n points2d.shrink_to_fit();\n indices.shrink_to_fit();\n int nInliers=indices.size();\n \n cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1);\n cv::Mat rVec,tVec,rMat;\n \n std::vector status(points3d.size());\n cv::solvePnPRansac(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec,false,1000,threshold,prob);\n cv::Rodrigues(rVec,rMat);\n \n for(int r1=0;r1<3;r1++){\n for(int r2=0;r2<3;r2++){\n pose.rotation(r1,r2)=rMat.at(r1,r2);\n }\n }\n \n tVec=-rMat.t()*tVec;\n assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1);\n pose.translation(0)=tVec.at(0);\n pose.translation(1)=tVec.at(1);\n pose.translation(2)=tVec.at(2);\n \n \n for (int i=0;i=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){\n \n Eigen::Vector3d proj=keyFrame1->mvLocalMapPoints[i].getPosition();\n proj=pose.rotation*(proj-pose.translation);\n proj/=proj(2);\n \n Eigen::Vector3d vec=keyFrame2->mvLocalMapPoints[matches12[i]].vec;\n Eigen::Vector3d error=proj-vec;\n \n if (error.norm()>threshold) {\n matches21[matches12[i]]=-1;\n matches12[i]=-1;\n nInliers--;\n }\n }\n }\n return nInliers;\n }\n}\n\n\n\n\n" }, { "path": "GSLAM/Estimation.h", "content": "//\n// Optimization.h\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"KLT.h\"\n#include \"KeyFrame.h\"\n\nnamespace GSLAM {\n \n class Undistortion{\n \n };\n \n class RelativeOutlierRejection{\n public:\n Eigen::Matrix3d K;\n Eigen::Matrix3d rotation;\n float theshold;\n float prob;\n double minViewAngle;\n double medViewAngle;\n vector* isValid;\n \n int relativeRansac(const KeyFrame* keyFrame,\n const KLT_FeatureList featureList);\n };\n \n class RelativePoseEstimation{\n public:\n \n bool rotationIsKnown;\n Eigen::Matrix3d rotation;\n Eigen::Vector3d translation;\n \n double medViewAngle;\n double minViewAngle;\n vector* isValid;\n \n int estimateRelativePose(const KeyFrame* keyFrame,\n const KLT_FeatureList featureList,\n std::vector &pVectors);\n void unitTest();\n private:\n \n };\n \n \n class LocalFactorization{\n public:\n void process(KeyFrame *keyFrame);\n void iterativeRefine(KeyFrame *keyFrame);\n };\n \n class LocalBundleAdjustment{\n \n public:\n \n double projErrorThres;\n double viewAngleThres;\n \n void refinePoints(KeyFrame* keyFrame);\n void triangulate2(KeyFrame* keyFrame);\n \n void bundleAdjust(KeyFrame *keyFrame,bool cameraFixed=false);\n void triangulate(KeyFrame *keyFrame);\n \n void refineKeyFrameConnection(KeyFrame *keyFrame);\n \n int refineKeyFrameMatches(KeyFrame* keyFrame1,const KeyFrame* keyFrame2,Transform& transform,\n std::vector& matches12,std::vector& matches21);\n \n //void estimateFarFrames(KeyFrame* keyFrame);\n \n int BAIterations;\n };\n \n class PnPEstimator{\n public:\n int minCount;\n double threshold;\n double prob;\n \n int estimate(KeyFrame *keyFrame1,KeyFrame *keyFrame2,\n const vector& matches,Transform& pose);\n \n int estimate(KeyFrame *keyFrame1,KeyFrame *keyFrame2,\n vector& matches12,vector& matches21,Transform& pose);\n \n };\n}" }, { "path": "GSLAM/Frame.cpp", "content": "//\n// Frame.cpp\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"Frame.h\"\n#include \"opencv2/imgproc/imgproc.hpp\"\n\nnamespace GSLAM{\n \n long unsigned int Frame::nNextId=0;\n bool Frame::mbInitialComputations=true;\n float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy;\n float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY;\n float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv;\n \n Frame::Frame(cv::Mat &im, const double &timeStamp,\n ORBextractor* extractor, ORBVocabulary* voc,\n cv::Mat &K, cv::Mat &distCoef)\n :mpORBvocabulary(voc),mpORBextractor(extractor),\n mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()){\n \n keyFramePtr=NULL;\n \n //im.copyTo(debugImage);\n#ifdef DEBUG\n //im.copyTo(debugImage);\n#endif\n \n // Exctract ORB\n (*mpORBextractor)(im,cv::Mat(),mvKeys,mDescriptors);\n \n N = mvKeys.size();\n \n if(mvKeys.empty())\n return;\n \n mvpLocalMapPoints = vector(N,static_cast(NULL));\n \n \n UndistortKeyPoints();\n \n // This is done for the first created Frame\n if(mbInitialComputations){\n \n ComputeImageBounds(im);\n \n \n mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)\n /static_cast(mnMaxX-mnMinX);\n \n mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)\n /static_cast(mnMaxY-mnMinY);\n \n fx = K.at(0,0);\n fy = K.at(1,1);\n cx = K.at(0,2);\n cy = K.at(1,2);\n \n mbInitialComputations=false;\n }\n \n \n mnId=nNextId++;\n \n //Scale Levels Info\n mnScaleLevels = mpORBextractor->GetLevels();\n mfScaleFactor = mpORBextractor->GetScaleFactor();\n mfLogScaleFactor = std::log(mfScaleFactor);\n \n \n mvScaleFactors.resize(mnScaleLevels);\n mvLevelSigma2.resize(mnScaleLevels);\n \n \n mvScaleFactors[0]=1.0f;\n mvLevelSigma2[0]=1.0f;\n for(int i=1; i(N,false);\n ComputeBoW();\n }\n \n bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY){\n \n posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);\n posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);\n \n //Keypoint's coordinates are undistorted, which could cause to go out of the image\n if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)\n return false;\n \n return true;\n }\n \n void Frame::ComputeImageBounds(const cv::Mat &im)\n {\n if(mDistCoef.at(0)!=0.0){\n \n cv::Mat mat(4,2,CV_32F);\n mat.at(0,0)=0.0; mat.at(0,1)=0.0;\n mat.at(1,0)=im.cols; mat.at(1,1)=0.0;\n mat.at(2,0)=0.0; mat.at(2,1)=im.rows;\n mat.at(3,0)=im.cols; mat.at(3,1)=im.rows;\n \n // Undistort corners\n mat=mat.reshape(2);\n cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);\n mat=mat.reshape(1);\n \n mnMinX = min(mat.at(0,0),mat.at(2,0));\n mnMaxX = max(mat.at(1,0),mat.at(3,0));\n mnMinY = min(mat.at(0,1),mat.at(1,1));\n mnMaxY = max(mat.at(2,1),mat.at(3,1));\n \n }else{\n \n mnMinX = 0.0f;\n mnMaxX = im.cols;\n mnMinY = 0.0f;\n mnMaxY = im.rows;\n }\n }\n \n void Frame::UndistortKeyPoints()\n {\n if(mDistCoef.at(0)==0.0){\n mvKeysUn=mvKeys;\n return;\n }\n \n // Fill matrix with points\n cv::Mat mat(N,2,CV_32F);\n for(int i=0; i(i,0)=mvKeys[i].pt.x;\n mat.at(i,1)=mvKeys[i].pt.y;\n }\n \n // Undistort points\n mat=mat.reshape(2);\n cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);\n mat=mat.reshape(1);\n \n // Fill undistorted keypoint vector\n mvKeysUn.resize(N);\n for(int i=0; i(i,0);\n kp.pt.y=mat.at(i,1);\n mvKeysUn[i]=kp;\n }\n }\n \n void Frame::ComputeBoW(){\n \n if(mBowVec.empty()){\n vector vCurrentDesc = toDescriptorVector(mDescriptors);\n mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);\n }\n }\n \n std::vector Frame::toDescriptorVector(const cv::Mat &Descriptors){\n \n std::vector vDesc;\n vDesc.reserve(Descriptors.rows);\n for (int j=0;j Frame::GetFeaturesInArea(const float &x, const float &y, const float &r,\n const int minLevel, const int maxLevel,vector &distances) const\n {\n vector vIndices;\n vIndices.reserve(N);\n distances.reserve(N);\n \n const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));\n if(nMinCellX>=FRAME_GRID_COLS)\n return vIndices;\n \n const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));\n if(nMaxCellX<0)\n return vIndices;\n \n const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));\n if(nMinCellY>=FRAME_GRID_ROWS)\n return vIndices;\n \n const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));\n if(nMaxCellY<0)\n return vIndices;\n \n const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);\n \n for(int ix = nMinCellX; ix<=nMaxCellX; ix++)\n {\n for(int iy = nMinCellY; iy<=nMaxCellY; iy++)\n {\n const vector vCell = mGrid[ix][iy];\n if(vCell.empty())\n continue;\n \n for(size_t j=0, jend=vCell.size(); j=0)\n if(kpUn.octave>maxLevel)\n continue;\n }\n \n const float distx = kpUn.pt.x-x;\n const float disty = kpUn.pt.y-y;\n \n if(fabs(distx) (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n#ifndef FRAME_H\n#define FRAME_H\n\n#include\n#include \"Settings.h\"\n#include \"Transform.h\"\n#include \"KeyFrame.h\"\n#include \"./DBoW2/BowVector.h\"\n#include \"./DBoW2/FeatureVector.h\"\n#include \"ORBVocabulary.h\"\n#include \"ORBextractor.h\"\n\n#include \n\n\nnamespace GSLAM\n{\n \n#define FRAME_GRID_ROWS 54\n#define FRAME_GRID_COLS 96\n\nclass LocalMapPoint;\nclass KeyFrame;\n \nclass Frame\n{\npublic:\n \n Frame(cv::Mat &im, const double &timeStamp, ORBextractor* extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef);\n // Copy constructor.\n Frame(const Frame &frame);\n\npublic:\n \n int mnId;\n KeyFrame* keyFramePtr;\n \n // Vocabulary used for relocalization.\n ORBVocabulary* mpORBvocabulary;\n \n // Feature extractor. The right is used only in the stereo case.\n ORBextractor* mpORBextractor;\n\n DBoW2::BowVector mBowVec;\n DBoW2::FeatureVector mFeatVec;\n \n int N;\n int NCoarse;\n \n cv::Mat mDescriptors;\n std::vector mvKeys;\n std::vector mvKeysUn;\n std::vector mvbOutlier;\n std::vector mvpLocalMapPoints;\n std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];\n \n // Frame timestamp.\n double mTimeStamp;\n \n //static members\n static float mnMinX;\n static float mnMaxX;\n static float mnMinY;\n static float mnMaxY;\n static bool mbInitialComputations;\n static long unsigned int nNextId;\n static float mfGridElementWidthInv;\n static float mfGridElementHeightInv;\n \n cv::Mat mK;\n static float fx;\n static float fy;\n static float cx;\n static float cy;\n static float invfx;\n static float invfy;\n cv::Mat mDistCoef;\n \n \n float mfOriginalScaleFacotr;\n \n // Scale pyramid info.\n int mnScaleLevels;\n float mfScaleFactor;\n float mfLogScaleFactor;\n vector mvScaleFactors;\n vector mvInvScaleFactors;\n vector mvLevelSigma2;\n vector mvInvLevelSigma2;\n \n bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);\n void ComputeImageBounds(const cv::Mat &im);\n void UndistortKeyPoints();\n void ComputeBoW();\n \n vector toDescriptorVector(const cv::Mat &Descriptors);\n vector GetFeaturesInArea(const float &x, const float &y, const float &r,\n const int minLevel, const int maxLevel,vector &distances) const;\n \n\n#ifdef DEBUG\n cv::Mat debugImage;\n#endif\n\n};\n\n}\n#endif\n\n\n" }, { "path": "GSLAM/Geometry.cpp", "content": "//\n// Geometry.cpp\n// GSLAM\n//\n// Created by ctang on 9/17/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \n#include \"Geometry.h\"\n#include \"ceres/ceres.h\"\n\nEigen::Vector3d multiviewTriangulationLinear(const std::vector &points,\n const std::vector &rotations,\n const std::vector &cameras){\n Eigen::Vector3d point3D;\n int ncamera=points.size();\n Eigen::MatrixX4d A=Eigen::MatrixX4d::Zero(3*ncamera,4);\n Eigen::MatrixX4d pLastRow=Eigen::MatrixX4d::Zero(ncamera,4);\n for(int i=0;i(0,0)=rotations[i];\n P.col(3)=-rotations[i]*cameras[i];\n A.block<3,4>(3*i,0)=crossMat*P;\n pLastRow.row(i)=P.row(2);\n }\n Eigen::Matrix4d AtA=A.transpose()*A;\n Eigen::JacobiSVD svd(AtA,Eigen::ComputeFullV);\n Eigen::Vector4d solution=svd.matrixV().col(3);\n \n point3D(0)=solution(0)/solution(3);\n point3D(1)=solution(1)/solution(3);\n point3D(2)=solution(2)/solution(3);\n \n Eigen::VectorXd product=pLastRow*solution;\n int positiveCount=0;\n for(int i=0;i0);\n }\n \n if(positiveCount& points,\n const std::vector& rotations,\n const std::vector& cameras){\n \n \n}*/\n\n\n\n\ndouble multiviewTriangulationDepth(const std::vector &points,\n const std::vector &rotations,\n const std::vector &cameras){\n \n \n int ncameras=points.size()-1;\n Eigen::VectorXd A=Eigen::VectorXd(3*ncameras);\n Eigen::VectorXd B=Eigen::VectorXd(3*ncameras);\n \n for (int i=1;i<=ncameras;i++) {\n Eigen::Matrix3d crossMat;\n Eigen::Vector3d rotated=rotations[i].transpose()*points[i];\n setCrossMatrix(crossMat,rotated);\n rotated=crossMat*points[0];\n A.block<3,1>(3*(i-1),0)=rotated;\n B.block<3,1>(3*(i-1),0)=crossMat*cameras[i];\n }\n \n double a=A.dot(A);\n double b=A.dot(B);\n return std::abs(b/a);\n}\n\n\n/*void multiviewTriangulationNonliear(double& depth,\n const std::vector& points,\n const std::vector& rotations,\n const std::vector& cameras){\n \n \n}*/\n\n\n\ninline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta,\n const double sintheta,\n const double wx,\n const double wy,\n const double wz){\n \n Eigen::Matrix3d R;\n R(0,0) = costheta + wx*wx*(1 - costheta);\n R(1,0) = wz*sintheta + wx*wy*(1 - costheta);\n R(2,0) = -wy*sintheta + wx*wz*(1 - costheta);\n R(0,1) = wx*wy*(1 - costheta) - wz*sintheta;\n R(1,1) = costheta + wy*wy*(1 - costheta);\n R(2,1) = wx*sintheta + wy*wz*(1 - costheta);\n R(0,2) = wy*sintheta + wx*wz*(1 - costheta);\n R(1,2) = -wx*sintheta + wy*wz*(1 - costheta);\n R(2,2) = costheta + wz*wz*(1 - costheta);\n return R;\n}\n\nEigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eigen::Vector3d &v2){\n double cos12=v1.dot(v2)/(v1.norm()*v2.norm());\n double sin12=sqrt(1-cos12*cos12);\n Eigen::Vector3d axis =v1.cross(v2);\n axis.normalize();\n Eigen::Matrix3d R=AngleAxis2RotationMatrix(cos12,sin12,axis(0),axis(1),axis(2));\n return R;\n}\n\n\n" }, { "path": "GSLAM/Geometry.h", "content": "//\n// Geometry.hpp\n// GSLAM\n//\n// Created by ctang on 9/17/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifndef Geometry_h\n#define Geometry_h\n\n#include \"Eigen/Dense\"\n#include \n\ninline void setCrossMatrix(Eigen::Matrix3d &crossMat,const Eigen::Vector3d& vector){\n crossMat(0,0)=crossMat(1,1)=crossMat(2,2)=0.0;\n crossMat(0,1)= vector(2);\n crossMat(0,2)= -vector(1);\n crossMat(1,0)= -vector(2);\n crossMat(1,2)= vector(0);\n crossMat(2,0)= vector(1);\n crossMat(2,1)= -vector(0);\n}\n\n\nEigen::Vector3d multiviewTriangulationLinear(const std::vector &points,\n const std::vector &rotations,\n const std::vector &cameras);\n\ndouble multiviewTriangulationDepth(const std::vector &points,\n const std::vector &rotations,\n const std::vector &cameras);\n\nvoid multiviewTriangulationNonlinear(double& depth,\n const std::vector &points,\n const std::vector &rotations,\n const std::vector &cameras);\n\n\ninline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta,\n const double sintheta,\n const double wx,\n const double wy,\n const double wz);\n\nEigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eigen::Vector3d &v2);\n\n#endif /* Geometry_h */\n" }, { "path": "GSLAM/GlobalReconstruction.cpp", "content": "\n//\n// GlobalReconstruction.cpp\n// GSLAM\n//\n// Created by ctang on 9/28/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n//#include \"opencv2/viz.hpp\"\n#include \"GlobalReconstruction.h\"\n#include \"KeyFrame.h\"\n#include \"Drawer.h\"\n#include \"opencv2/highgui/highgui.hpp\"\n#include \"opencv2/imgproc/imgproc.hpp\"\n#include \"ceres/ceres.h\"\n\n#include \"theia/sfm/global_pose_estimation/robust_rotation_estimator.h\"\n#include \"theia/sfm/twoview_info.h\"\n#include \"theia/sfm/types.h\"\n#include \n\nconst bool disable_loop=true;\n\nnamespace GSLAM {\n \n\n void GlobalReconstruction::getScaleConstraint(KeyFrame* keyFrame1,\n vector& scaleConstraints){\n \n for(map >::iterator mit=keyFrame1->mConnectedKeyFrameMatches.begin(),\n mend=keyFrame1->mConnectedKeyFrameMatches.end();\n mit!=mend;\n mit++){\n \n KeyFrame* keyFrame2=mit->first;\n \n if (keyFrame2->mvLocalFrames.empty()) {\n continue;\n }\n \n Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];\n ScaleConstraint constraint;\n constraint.keyFrameIndex1=keyFrame1->mnId;\n constraint.keyFrameIndex2=keyFrame2->mnId;\n vector scales;\n vector& matches=mit->second;\n \n for (int i=0;i=0) {\n \n assert(keyFrame1->mvLocalMapPoints[i].isEstimated);\n if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated){\n continue;\n }\n \n Eigen::Vector3d distance1=keyFrame1->mvLocalMapPoints[i].getPosition()-pose.translation;\n Eigen::Vector3d distance2=keyFrame2->mvLocalMapPoints[matches[i]].getPosition();\n \n /*printf(\"%d %d %x %x\\n\",keyFrame1->outId,keyFrame2->outId,\n keyFrame1->mvLocalMapPoints[i].gMP,keyFrame2->mvLocalMapPoints[matches[i]].gMP);\n \n for(map::iterator mit=keyFrame1->mvLocalMapPoints[i].gMP->measurements.begin(),\n mend=keyFrame1->mvLocalMapPoints[i].gMP->measurements.end();\n mit!=mend; mit++){\n printf(\"%d %d\\n\",mit->first->outId,mit->second);\n }\n \n for(map::iterator mit=keyFrame2->mvLocalMapPoints[matches[i]].gMP->measurements.begin(),\n mend=keyFrame2->mvLocalMapPoints[matches[i]].gMP->measurements.end();\n mit!=mend; mit++){\n printf(\"%d %d\\n\",mit->first->outId,mit->second);\n }*/\n \n //assert(keyFrame1->mvLocalMapPoints[i].gMP==keyFrame2->mvLocalMapPoints[matches[i]].gMP);\n double relativeScale=distance2.norm()/distance1.norm();\n scales.push_back(relativeScale);\n }\n }\n \n if (scales.size()>=scaleThreshold) {\n sort(scales.begin(),scales.end());\n constraint.value12=log(scales[scales.size()/2]);\n }else{\n continue;\n }\n \n constraint.weight=1.0;\n scaleConstraints.push_back(constraint);\n }\n }\n \n void GlobalReconstruction::getRotationConstraint(KeyFrame* keyFrame1,\n vector& rotationConstraints){\n \n for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),\n mend=keyFrame1->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n KeyFrame* keyFrame2=mit->first;\n Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];\n RotationConstraint constraint;\n constraint.keyFrameIndex1=keyFrame1->mnId;\n constraint.keyFrameIndex2=keyFrame2->mnId;\n constraint.rotation12=pose.rotation;\n if(pose.scale==-1&&(keyFrame1->frameId<70||keyFrame2->frameId<70)){\n continue;\n }\n \n //printf(\"add %d %d\\n\",constraint.keyFrameIndex1,constraint.keyFrameIndex2);\n rotationConstraints.push_back(constraint);\n }\n }\n \n void GlobalReconstruction::getTranslationConstraint(KeyFrame* keyFrame1,\n vector& translationConstraints){\n \n for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),\n mend=keyFrame1->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n KeyFrame* keyFrame2=mit->first;\n Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];\n TranslationConstraint constraint;\n constraint.keyFrameIndex1=keyFrame1->mnId;\n constraint.keyFrameIndex2=keyFrame2->mnId;\n constraint.rotation1=keyFrame1->pose.rotation;\n constraint.translation12=pose.translation/keyFrame1->scale;\n if(pose.scale==-1&&(keyFrame1->frameId<70||keyFrame2->frameId<70)){\n continue;\n }\n translationConstraints.push_back(constraint);\n }\n }\n \n void GlobalReconstruction::getSIM3Constraint(KeyFrame* keyFrame1,vector& sim3Constraints){\n \n for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),\n mend=keyFrame1->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n KeyFrame* keyFrame2=mit->first;\n Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];\n SIM3Constraint constraint;\n constraint.keyFrameIndex1=keyFrame1->mnId;\n constraint.keyFrameIndex2=keyFrame2->mnId;\n constraint.rotation12=pose.rotation;\n constraint.translation12=pose.translation;\n constraint.weight=1.0;\n \n vector& matches=keyFrame1->mConnectedKeyFrameMatches[keyFrame2];\n vector scales;\n for (int i=0;i=0) {\n assert(keyFrame1->mvLocalMapPoints[i].isEstimated);\n if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated){\n continue;\n }\n Eigen::Vector3d distance1=keyFrame1->mvLocalMapPoints[i].getPosition()-pose.translation;\n Eigen::Vector3d distance2=keyFrame2->mvLocalMapPoints[matches[i]].getPosition();\n double relativeScale=distance2.norm()/distance1.norm();\n scales.push_back(relativeScale);\n }\n }\n \n if(scales.empty()){\n continue;\n }\n \n sort(scales.begin(),scales.end());\n constraint.scale12=scales[scales.size()/2];\n \n sim3Constraints.push_back(constraint);\n }\n }\n \n void GlobalReconstruction::estimateScale(){\n \n vector scaleIndex(keyFrames.size(),-1);\n vector scaleConstraints;\n \n for (int i=0;imnId==i);\n getScaleConstraint(keyFrames[i],scaleConstraints);\n }\n //printf(\"%d keyframe size\\n\",keyFrames.size());\n int nScales=0;\n for (int i=0;i newScales;\n newScales.resize(nScales);\n for(int i=0;ilogScale;\n }\n }\n /*for (int i=0;ilogScale=newScales[scaleIndex[i]];\n keyFrames[i]->scale=exp(keyFrames[i]->logScale);\n printf(\"%f\\n\",keyFrames[i]->scale);\n }\n \n static ofstream record(\"/Users/chaos/Desktop/debug/scales_error.txt\");\n for (int i=0;ilogScale-keyFrames[scaleConstraints[i].keyFrameIndex1]->logScale;\n diff=abs(diff-scaleConstraints[i].value12);\n int id1=keyFrames[scaleConstraints[i].keyFrameIndex1]->outId;\n int id2=keyFrames[scaleConstraints[i].keyFrameIndex2]->outId;\n //printf(\"%f %d %d\\n\",diff,id1,id2);\n record< &rotationIndex){\n \n std::vector rotationConstraints(0);\n std::vector newRotations;\n \n int nRotations=0;\n rotationIndex.resize(keyFrames.size(),-1);\n for (int k=0;kpose.rotation);\n }\n \n if (rotationIndex[rotationConstraints[i].keyFrameIndex2]==-1) {\n rotationIndex[rotationConstraints[i].keyFrameIndex2]=nRotations;\n nRotations++;\n newRotations.push_back(keyFrames[rotationConstraints[i].keyFrameIndex2]->pose.rotation);\n }\n \n \n rotationConstraints[i].variableIndex1=rotationIndex[rotationConstraints[i].keyFrameIndex1];\n rotationConstraints[i].variableIndex2=rotationIndex[rotationConstraints[i].keyFrameIndex2];\n }\n }\n \n std::unordered_map view_pairs;\n std::unordered_map orientations;\n \n theia::RobustRotationEstimator::Options options;\n options.max_num_irls_iterations=100;\n options.max_num_l1_iterations=10;\n theia::RobustRotationEstimator rotation_estimator(options);\n \n for (int i=0;imvLocalFrames.back().pose.rotation*newRotations[i-1];\n }\n \n \n for (int i=0;ipose.rotation=newRotations[rotationIndex[k]];\n }\n std::cout<<\"rotation end\"< &rotationIndex){\n \n vector rotationConstraints(0);\n vector newRotations(keyFrames.size());\n\n for (int k=0;kpose.rotation;\n }\n \n RotationConstraint constraint;\n constraint.keyFrameIndex1=-1;\n constraint.keyFrameIndex2=0;\n constraint.variableIndex1=-1;\n constraint.variableIndex2=0;\n constraint.rotation1=Eigen::Matrix3d::Identity();\n constraint.rotation12=Eigen::Matrix3d::Identity();\n constraint.weight=1.0;\n rotationConstraints.push_back(constraint);\n \n \n globalRotationEstimation.maxOuterIterations=1000;\n globalRotationEstimation.maxInnerIterations=20;\n globalRotationEstimation.solve(rotationConstraints,newRotations);\n \n for (int i=1;ipose.rotation=newRotations[rotationIndex[k]];\n }\n }\n \n void GlobalReconstruction::estimateTranslation(const vector &translationIndex){\n \n vector translationConstraints;\n for (int k=0;k newTranslations(keyFrames.size(),Eigen::Vector3d::Zero());\n globalTranslationEstimation.maxIterations=10000;\n globalTranslationEstimation.solve(translationConstraints,newTranslations);\n \n for (int i=1;ipose.translation=newTranslations[translationIndex[k]];\n cout<pose.translation.transpose()<pose.translation-keyFrames[translationConstraints[i].keyFrameIndex1]->pose.translation);\n \n double diff=differror.norm();\n \n int id1=keyFrames[translationConstraints[i].keyFrameIndex1]->outId;\n int id2=keyFrames[translationConstraints[i].keyFrameIndex2]->outId;\n \n record<\n bool operator()(const T* const camera,\n const T* const point,\n T* residuals) const {\n \n \n \n T transformed[3];\n ceres::AngleAxisRotatePoint(camera,point,transformed);\n \n transformed[0]+=camera[3];\n transformed[1]+=camera[4];\n transformed[2]+=camera[5];\n \n \n T predicted_x=transformed[0]/transformed[2];\n T predicted_y=transformed[1]/transformed[2];\n \n residuals[0]=predicted_x-(T)tracked_x;\n residuals[1]=predicted_y-(T)tracked_y;\n \n residuals[0]=residuals[0];\n residuals[1]=residuals[1];\n \n return true;\n }\n \n static ceres::CostFunction* Create(const double tracked_x,\n const double tracked_y) {\n return (new ceres::AutoDiffCostFunction(new GlobalError2(tracked_x,tracked_y)));\n };\n \n double tracked_x;\n double tracked_y;\n };\n void GlobalReconstruction::globalRefine(){\n \n std::vector points;\n std::vector cameras;\n std::vector> pairCameraPoint;\n \n int nPoints=0;\n cameras.resize(6*keyFrames.size());\n for (int i=0;ipose.rotation*keyFrame->pose.translation;\n ceres::RotationMatrixToAngleAxis(keyFrame->pose.rotation.data(),\n &cameras[6*i]);\n cameras[6*i+3]=translation(0);\n cameras[6*i+4]=translation(1);\n cameras[6*i+5]=translation(2);\n \n nPoints+=keyFrame->mvLocalMapPoints.size();\n }\n points.resize(3*nPoints);\n nPoints=0;\n \n ceres::Problem problem;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(0.003);\n \n \n for (int k=0;kmnId==k);\n for (int i=0;imvLocalMapPoints.size();i++) {\n if (keyFrame1->mvLocalMapPoints[i].isEstimated) {\n \n Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();\n point3D/=keyFrame1->scale;\n point3D=keyFrame1->pose.rotation.transpose()*point3D+keyFrame1->pose.translation;\n \n points[3*(nPoints+i)]=point3D(0);\n points[3*(nPoints+i)+1]=point3D(1);\n points[3*(nPoints+i)+2]=point3D(2);\n }\n }\n std::vector status(keyFrame1->mvLocalMapPoints.size(),false);\n for(std::map >::iterator mit=keyFrame1->mConnectedKeyFrameMatches.begin(),\n mend=keyFrame1->mConnectedKeyFrameMatches.end();\n mit!=mend;\n mit++){\n \n KeyFrame* keyFrame2=mit->first;\n if (keyFrame2->mvLocalFrames.empty()) {\n continue;\n }\n Transform transform=keyFrame1->mConnectedKeyFramePoses[keyFrame2];\n \n if (transform.scale<-10.0&&disable_loop) {\n continue;\n }\n \n std::vector& matches=mit->second;\n assert(matches.size()==keyFrame1->mvLocalMapPoints.size());\n \n for (int i=0;i=0) {\n \n assert(keyFrame1->mvLocalMapPoints[i].isEstimated);\n \n Eigen::Vector3d projection(&points[3*(nPoints+i)]);\n projection=keyFrame2->pose.rotation*(projection-keyFrame2->pose.translation);\n projection/=projection(2);\n projection-=keyFrame2->mvLocalMapPoints[matches[i]].vec;\n \n \n if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated&&projection.norm()>0.008){\n continue;\n }\n ceres::CostFunction* cost_function = GlobalError2::Create(keyFrame2->mvLocalMapPoints[matches[i]].vec(0),\n keyFrame2->mvLocalMapPoints[matches[i]].vec(1));\n problem.AddResidualBlock(cost_function,loss_function,\n &cameras[6*keyFrame2->mnId],\n &points[3*(nPoints+i)]);\n status[i]=true;\n }\n }\n }\n \n if(keyFrame1->nextKeyFramePtr!=NULL){\n for(int i=0;imvLocalMapPoints.size();i++){\n \n Eigen::Vector3d projection(&points[3*(nPoints+i)]);\n projection=keyFrame1->nextKeyFramePtr->pose.rotation*(projection-keyFrame1->nextKeyFramePtr->pose.translation);\n projection/=projection(2);\n projection-=(*keyFrame1->mvLocalMapPoints[i].vecs.back());\n \n if (keyFrame1->mvLocalMapPoints[i].isEstimated\n &&keyFrame1->mvLocalMapPoints[i].vecs.back()!=NULL&&projection.norm()<0.008) {\n assert(keyFrame1->mvLocalMapPoints[i].vecs.size()==keyFrame1->mvLocalFrames.size());\n Eigen::Vector3d projection=(*keyFrame1->mvLocalMapPoints[i].vecs.back());\n projection/=projection(2);\n \n ceres::CostFunction* cost_function = GlobalError2::Create(projection(0),projection(1));\n problem.AddResidualBlock(cost_function,loss_function,\n &cameras[6*keyFrame1->nextKeyFramePtr->mnId],\n &points[3*(nPoints+i)]);\n status[i]=true;\n }\n }\n }\n \n for (int i=0;imvLocalMapPoints.size();i++) {\n if (status[i]==true) {\n \n ceres::CostFunction* cost_function = GlobalError2::Create(keyFrame1->mvLocalMapPoints[i].vec(0),\n keyFrame1->mvLocalMapPoints[i].vec(1));\n problem.AddResidualBlock(cost_function,loss_function,\n &cameras[6*keyFrame1->mnId],\n &points[3*(nPoints+i)]);\n keyFrame1->mvLocalMapPoints[i].isEstimated=true;\n }else{\n keyFrame1->mvLocalMapPoints[i].isEstimated=false;\n }\n }\n nPoints+=keyFrame1->mvLocalMapPoints.size();\n }\n \n \n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_SCHUR;\n options.minimizer_progress_to_stdout = true;\n options.max_num_iterations=100;\n \n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n \n for(int i=0;ipose.rotation=rotation;\n keyFrames[i]->pose.translation=translation;\n }\n \n \n \n nPoints=0;\n for (int k=0;k scales;\n for (int i=0;imvLocalMapPoints.size();i++) {\n if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) {\n keyFrames[k]->mvLocalMapPoints[i].globalPosition(0)=points[3*(nPoints+i)];\n keyFrames[k]->mvLocalMapPoints[i].globalPosition(1)=points[3*(nPoints+i)+1];\n keyFrames[k]->mvLocalMapPoints[i].globalPosition(2)=points[3*(nPoints+i)+2];\n \n Eigen::Vector3d globalDistance=keyFrames[k]->mvLocalMapPoints[i].globalPosition\n -keyFrames[k]->pose.translation;\n Eigen::Vector3d localDistance=keyFrames[k]->mvLocalMapPoints[i].getPosition();\n scales.push_back(localDistance.norm()/globalDistance.norm());\n }\n }\n std::sort(scales.begin(),scales.end());\n keyFrames[k]->scale=scales[scales.size()/2];\n nPoints+=keyFrames[k]->mvLocalMapPoints.size();\n }\n \n for(int i=1;ipose.rotation=keyFrames[i]->pose.rotation*keyFrames[0]->pose.rotation.transpose();\n keyFrames[i]->pose.translation=keyFrames[0]->pose.rotation*(keyFrames[i]->pose.translation\n -keyFrames[0]->pose.translation);\n }\n \n for (int k=0;kmvLocalMapPoints.size();i++) {\n if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) {\n keyFrames[k]->mvLocalMapPoints[i].globalPosition=keyFrames[0]->pose.rotation*\n (keyFrames[k]->mvLocalMapPoints[i].globalPosition-keyFrames[0]->pose.translation);\n }\n }\n }\n \n keyFrames[0]->pose.rotation=Eigen::Matrix3d::Identity();\n keyFrames[0]->pose.translation=Eigen::Vector3d::Zero();\n }\n \n \n \n void GlobalReconstruction::addNewKeyFrame(KeyFrame* keyFrame){\n keyFrames.push_back(keyFrame);\n }\n \n void GlobalReconstruction::savePly(){\n \n ofstream results(path+std::string(\"/keyframes.txt\"));\n for (int k=0;kframeId<<' '<pose.translation.transpose();\n for (int i1=0;i1<3;i1++) {\n for (int i2=0;i2<3;i2++) {\n results<<' '<pose.rotation(i1,i2);\n }\n }\n results<mvLocalFrames.size();i++) {\n \n Eigen::Matrix3d rotation;\n rotation=keyFrames[k]->mvLocalFrames[i].pose.rotation*keyFrames[k]->pose.rotation;\n\n \n Eigen::Vector3d translation=keyFrames[k]->mvLocalFrames[i].pose.translation;\n translation=keyFrames[k]->pose.rotation.transpose()*translation;\n translation/=keyFrames[k]->scale;\n translation+=keyFrames[k]->pose.translation;\n \n trajectories<mvLocalFrames[i].frameId<<' '<mvLocalMapPoints.size();i++) {\n keyFrames[k]->mvLocalMapPoints[i].isEstimated=false;\n }\n \n for(map >::iterator mit=keyFrames[k]->mConnectedKeyFrameMatches.begin(),\n mend=keyFrames[k]->mConnectedKeyFrameMatches.end();\n mit!=mend;\n mit++){\n \n for (int i=0;isecond.size();i++) {\n if (mit->second[i]>=0) {\n keyFrames[k]->mvLocalMapPoints[i].isEstimated=true;\n npoints++;\n }\n }\n }\n }\n \n ofstream pointcloud(path+std::string(\"/pointcloud.ply\"));\n pointcloud << \"ply\"\n << '\\n' << \"format ascii 1.0\"\n << '\\n' << \"element vertex \" <mvLocalMapPoints.size();i++) {\n if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){\n \n Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition();\n point3D/=keyFrames[k]->scale;\n point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation;\n \n uchar* color=keyFrames[k]->mvLocalMapPoints[i].color;\n pointcloud <=keyFrames.back()->mvLocalFrames.back().frameId){\n break;\n }\n \n cv::Mat image;\n if(drawer.mFrameIndexmvLocalFrames.back().frameId){\n drawer.getCurrentOpenGLCameraMatrix(Twc);\n \n// char name[200];\n// sprintf(name,\"%s/1080/frame%05d.pgm\",path,drawer.mFrameIndex+frameStart+1);\n// printf(name);\n// image=cv::imread(name);\n// printf(\"image %d %d\\n\",image.cols,image.rows);\n// if ((drawer.mFrameIndex)==keyFrames[drawer.mKeyFrameIndex]->frameId) {\n// cv::RNG rng(-1);\n// for(int i=0;imvLocalMapPoints.size();i++){\n// if(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount<=0){\n// continue;\n// }\n// //printf(\"%f %f %f\\n\",color.val[0],color.val[1],color[2]);\n// uchar *color=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].color;\n//\n// int Size=min(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount,20);\n// Size=max(Size,3);\n// cv::circle(image,\n// keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[0],Size,\n// CV_RGB(245,211,40),2,CV_AA);\n//\n// }\n// }else{\n// for(int i=0;imvLocalMapPoints.size();i++){\n//\n// if(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount<=0){\n// continue;\n// }\n//\n//\n// if((drawer.mFrameIndex-keyFrames[drawer.mKeyFrameIndex]->frameId)\n// >=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked.size()){\n// continue;\n// }\n// int localIndex=drawer.mFrameIndex-keyFrames[drawer.mKeyFrameIndex]->frameId;\n//\n// int Size=min(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount,20);\n// Size=max(Size,3);\n// uchar *color=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].color;\n// cv::circle(image,keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[localIndex],Size,\n// CV_RGB(245,211,40),2,CV_AA);\n//\n// for (int j=localIndex;j>max(localIndex-5,0);j--) {\n// cv::line(image,\n// keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[j],\n// keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[j-1],\n// CV_RGB(245,211,40),2,CV_AA);\n// }\n// }\n// }\n// cv::resize(image,image,cv::Size(720,405));\n// image.copyTo(preImage);\n s_cam.Follow(Twc);\n }else{\n preImage.copyTo(image);\n s_cam.Follow(Twc);\n }\n \n \n glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);\n d_cam.Activate(s_cam);\n glClearColor(1.0f,1.0f,1.0f,1.0f);\n \n drawer.drawCurrentCamera(Twc);\n drawer.drawKeyFrames();\n drawer.drawPoints();\n \n// char name[200];\n// sprintf(name,\"%s/viewer/viewr%05d\",path,drawer.mFrameIndex);\n// pangolin::SaveWindowOnRender(name);\n// sprintf(name,\"%s/viewer/track%05d.png\",path,drawer.mFrameIndex);\n// cv::imwrite(name,image);\n// image.release();\n \n pangolin::FinishFrame();\n }\n }\n bool myfunction (const LocalMapPoint* p1,const LocalMapPoint* p2) { return (p1->invdepthinvdepth); }\n \n void GlobalReconstruction::topview(){\n \n /*for (int k=0;k localMapPoints;\n for (int i=0;imvLocalMapPoints.size();i++) {\n if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) {\n localMapPoints.push_back(&keyFrames[k]->mvLocalMapPoints[i]);\n }\n }\n sort(localMapPoints.begin(),localMapPoints.end(),myfunction);\n \n for (int i=0;i<(0.8*localMapPoints.size());i++) {\n localMapPoints[i]->isEstimated=false;\n }\n }*/\n //getchar();\n //pangolin::WindowInterface& window=pangolin::CreateWindowAndBind(\"GSLAM: Map Viewer\",720,720);\n //window.Move(100,480);\n // 3D Mouse handler requires depth testing to be enabled\n //glEnable(GL_DEPTH_TEST);\n \n // Issue specific OpenGl we might need\n //glEnable (GL_BLEND);\n //glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);\n \n //pangolin::CreatePanel(\"menu\").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));\n \n float mViewpointX=0;\n float mViewpointY=0;\n float mViewpointZ=-1.8;\n float mViewpointF=500;\n \n for(int i=0;ipose.translation(0);\n mViewpointY+=keyFrames[i]->pose.translation(1);\n }\n mViewpointX/=keyFrames.size();\n mViewpointY/=keyFrames.size();\n \n \n float maxdistance=0.0;\n for (int i=1;ipose.translation.norm();\n if (distance>maxdistance) {\n maxdistance=distance;\n }\n }\n \n printf(\"%f %f\\n\",mViewpointX,mViewpointY);\n // Define Camera Render Object (for view / scene browsing)\n pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),\n pangolin::ModelViewLookAt(0,\n -maxdistance,\n 0,\n 0,0.0,0,\n pangolin::AxisDirection::AxisZ));\n \n // Add named OpenGL viewport to window and provide 3D Handler\n pangolin::View& d_cam = pangolin::CreateDisplay()\n .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)\n .SetHandler(new pangolin::Handler3D(s_cam));\n \n pangolin::OpenGlMatrix Twc;\n Twc.SetIdentity();\n \n bool bFollow = true;\n bool bLocalizationMode = false;\n float mT = 1e3/30.0;\n Drawer drawer;\n drawer.mCameraSize=0.5;\n drawer.mCameraLineWidth=3;\n drawer.mFrameIndex=0;\n drawer.mKeyFrameIndex=0;\n drawer.keyFrames=keyFrames;\n drawer.mGraphLineWidth=5.0;\n \n cv::namedWindow(\"track\");\n cv::moveWindow(\"track\",720,0);\n \n \n drawer.preTwc.resize(1);\n drawer.preTwc[0];\n drawer.preTwc[0].m[12]=0.0;\n drawer.preTwc[0].m[13]=0.0;\n drawer.preTwc[0].m[14]=0.0;\n \n pangolin::OpenGlMatrix curretMatrix;\n while(1){\n \n if(drawer.mFrameIndexmvLocalFrames.back().frameId){\n drawer.getCurrentOpenGLCameraMatrix(Twc);\n }else{\n break;\n }\n \n curretMatrix=s_cam.GetModelViewMatrix();\n\n \n glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);\n d_cam.Activate(s_cam);\n glClearColor(1.0f,1.0f,1.0f,1.0f);\n \n drawer.drawCurrentCamera(Twc);\n drawer.drawKeyFrames();\n drawer.drawPoints();\n \n char name[200];\n //sprintf(name,\"%s/viewer/top%05d\",path,drawer.mFrameIndex);\n //pangolin::SaveWindowOnRender(name);\n pangolin::FinishFrame();\n \n }\n \n drawer.mFrameIndex=0;\n drawer.mKeyFrameIndex=0;\n drawer.preTwc.resize(1);\n drawer.preTwc[0];\n drawer.preTwc[0].m[12]=0.0;\n drawer.preTwc[0].m[13]=0.0;\n drawer.preTwc[0].m[14]=0.0;\n while(1){\n \n if(drawer.mFrameIndexmvLocalFrames.back().frameId){\n drawer.getCurrentOpenGLCameraMatrix(Twc);\n }\n \n s_cam.SetModelViewMatrix(curretMatrix);\n \n glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);\n d_cam.Activate(s_cam);\n glClearColor(1.0f,1.0f,1.0f,1.0f);\n \n drawer.drawCurrentCamera(Twc);\n drawer.drawKeyFrames();\n drawer.drawPoints();\n \n// char name[200];\n// sprintf(name,\"%s/viewer/top%05d\",path,drawer.mFrameIndex);\n// pangolin::SaveWindowOnRender(name);\n pangolin::FinishFrame();\n \n }\n }\n}\n" }, { "path": "GSLAM/GlobalReconstruction.h", "content": "//\n// GlobalReconstruction.h\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifndef GlobalReconstruction_h\n#define GlobalReconstruction_h\n#include \n#include \"Eigen/Dense\"\n#include \"ceres/rotation.h\"\n\nnamespace GSLAM{\n \n inline Eigen::Vector3d ComputeRelativeRotationError(const Eigen::Matrix3d& relative_rotation_matrix,\n const Eigen::Matrix3d& rotation_matrix1,\n const Eigen::Matrix3d& rotation_matrix2) {\n // Compute the relative rotation error.\n const Eigen::Matrix3d relative_rotation_matrix_error =rotation_matrix2.transpose()*relative_rotation_matrix*rotation_matrix1;\n Eigen::Vector3d relative_rotation_error;\n ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(relative_rotation_matrix_error.data()),\n relative_rotation_error.data());\n return relative_rotation_error;\n }\n \n inline void ApplyRotation(const Eigen::Vector3d& rotation_change,Eigen::Matrix3d& rotation_matrix) {\n // Convert to rotation matrices.\n Eigen::Matrix3d rotation_change_matrix;\n ceres::AngleAxisToRotationMatrix(rotation_change.data(),ceres::ColumnMajorAdapter3x3(rotation_change_matrix.data()));\n // Apply the rotation change.\n rotation_matrix = rotation_matrix*rotation_change_matrix;\n }\n \n inline int max3(int a, int b, int c){\n int m = a;\n (m < b) && (m = b); //these are not conditional statements.\n (m < c) && (m = c); //these are just boolean expressions.\n return m;\n }\n \n \n typedef struct{\n \n int variableIndex1;\n int variableIndex2;\n \n int keyFrameIndex1;\n int keyFrameIndex2;\n \n double value1;\n double value12;\n double weight;\n \n bool isLoop;\n \n }ScaleConstraint;\n \n typedef struct{\n \n int variableIndex1;\n int variableIndex2;\n \n int keyFrameIndex1;\n int keyFrameIndex2;\n \n Eigen::Matrix3d rotation1;//if 1 is fixed\n Eigen::Matrix3d rotation12;\n \n double weight;\n bool isLoop;\n \n }RotationConstraint;\n \n typedef struct{\n \n int variableIndex1;\n int variableIndex2;\n \n int keyFrameIndex1;\n int keyFrameIndex2;\n \n Eigen::Matrix3d rotation1;\n Eigen::Vector3d translation1;//if 1 is fixed\n Eigen::Vector3d translation12;\n \n double weight;\n bool isLoop;\n \n }TranslationConstraint;\n \n typedef struct{\n \n int variableIndex1;\n int variableIndex2;\n \n int keyFrameIndex1;\n int keyFrameIndex2;\n \n Eigen::Matrix3d rotation12;\n Eigen::Vector3d translation12;\n double scale12;\n \n double weight;\n bool isLoop;\n \n }SIM3Constraint;\n \n \n class GlobalScaleEstimation{\n \n public:\n \n double\t*objective;\n int \t*rowStart;\n int \t*column;\n double\t*rowUpper;\n double\t*rowLower;\n double\t*columnUpper;\n double\t*columnLower;\n double \t*elementByRow;\n \n int maxIterations;\n \n GlobalScaleEstimation(){\n objective=NULL;\n rowStart=NULL;\n column=NULL;\n rowUpper=NULL;\n rowLower=NULL;\n columnUpper=NULL;\n columnLower=NULL;\n elementByRow=NULL;\n }\n \n void solve(const std::vector &constraints,std::vector &results);\n \n void test();\n };\n \n class GlobalRotationEstimation{\n \n public:\n \n double\t*objective;\n int \t*rowStart;\n int \t*column;\n double\t*rowUpper;\n double\t*rowLower;\n double\t*columnUpper;\n double\t*columnLower;\n double \t*elementByRow;\n \n int maxOuterIterations;\n int maxInnerIterations;\n \n GlobalRotationEstimation(){\n objective=NULL;\n rowStart=NULL;\n column=NULL;\n rowUpper=NULL;\n rowLower=NULL;\n columnUpper=NULL;\n columnLower=NULL;\n elementByRow=NULL;\n }\n \n void solve(const std::vector &constraints,std::vector& results);\n \n void test();\n \n private:\n \n };\n \n class GlobalTranslationEstimation{\n \n public:\n \n double\t*objective;\n int \t*rowStart;\n int \t*column;\n double\t*rowUpper;\n double\t*rowLower;\n double\t*columnUpper;\n double\t*columnLower;\n double \t*elementByRow;\n \n int maxIterations;\n \n GlobalTranslationEstimation(){\n objective=NULL;\n rowStart=NULL;\n column=NULL;\n rowUpper=NULL;\n rowLower=NULL;\n columnUpper=NULL;\n columnLower=NULL;\n elementByRow=NULL;\n }\n \n void solve(const std::vector &constraints,std::vector& results);\n \n void solveComplex(const std::vector &constraints,std::vector& results);\n \n void test();\n };\n \n class KeyFrame;\n \n class GlobalReconstruction{\n \n std::vector keyFrames;\n GlobalRotationEstimation globalRotationEstimation;\n GlobalScaleEstimation globalScaleEstimation;\n GlobalTranslationEstimation globalTranslationEstimation;\n \n void getScaleConstraint(KeyFrame* keyFrame1,std::vector& scaleConstraints);\n void getRotationConstraint(KeyFrame* keyFrame1,std::vector& rotationConstraints);\n void getTranslationConstraint(KeyFrame* keyFrame1,std::vector& translationConstraints);\n void getSIM3Constraint(KeyFrame* keyFrame1,std::vector& constraints);\n \n \n public:\n void saveTUM(const char* savename,const std::vector& timestamps);\n void globalRefine();\n char *path;\n int frameStart;\n GlobalReconstruction(){\n keyFrames.clear();\n }\n \n int scaleThreshold;\n void addNewKeyFrame(KeyFrame* keyFrame);\n void estimateScale();\n void estimateRotation(std::vector& rotationIndex);\n void estimateRotationRobust(const std::vector& rotationIndex);\n void estimateTranslation(const std::vector& translationIndex);\n void estimateSIM3();\n void savePly();\n void visualize();\n void topview();\n };\n}\n\n#endif /* GlobalReconstruction_h */\n" }, { "path": "GSLAM/GlobalRotationEstimation.cpp", "content": "//\n// GlobalRotationEstimation.cpp\n// GSLAM\n//\n// Created by ctang on 10/2/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"GlobalReconstruction.h\"\n#include \"L1Solver.h\"\n#include \"opencv2/core/core.hpp\"\n#include \n\n\nnamespace GSLAM{\n \n \n void GlobalRotationEstimation::solve(const std::vector& constraints,\n std::vector& results){\n \n static const double kConvergenceThreshold = 1e-2;\n \n int variableCount=results.size();\n int constraintCount=constraints.size();\n int rowCount=2*constraintCount;\n \n int columnCount=variableCount+constraintCount;\n int singleVariableConstraintCount=0;\n \n for(int i=0;i rotationErrors(constraintCount);\n std::vector rotationUpdates(variableCount);\n \n double rotationError=0.0;\n for (int iter=0;iter globalRotations(num_rotations);\n std::vector constraints;\n cv::RNG rng(-1);\n for (int i=0;inum_rotations-100) {\n constraint.variableIndex1=0;\n constraint.variableIndex2=i;\n constraint.rotation12=globalRotations[i]*globalRotations[0].transpose();\n constraint.weight=1.0;\n constraints.push_back(constraint);\n }\n }\n \n for (int i=0;i results(6,Eigen::Matrix3d::Identity());\n int i=0;\n std::vector incrementalConstraints;\n \n for (int f=5;fsolve(incrementalConstraints,results);\n }else{\n std::vector wx,wy,wz;\n for (int j=incrementalConstraints.size()-1;j>0;j--) {\n if (incrementalConstraints[j].variableIndex2==f) {\n const Eigen::Matrix3d rotationf=incrementalConstraints[j].rotation12*results[incrementalConstraints[j].variableIndex1];\n double angles[3];\n ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(rotationf.data()),angles);\n wx.push_back(angles[0]);\n wy.push_back(angles[1]);\n wz.push_back(angles[2]);\n }\n }\n std::sort(wx.begin(),wx.end());\n std::sort(wy.begin(),wy.end());\n std::sort(wz.begin(),wz.end());\n \n const double angles[3]={wx[wx.size()/2],wy[wy.size()/2],wz[wz.size()/2]};\n Eigen::Matrix3d rotationf;\n ceres::AngleAxisToRotationMatrix(angles,ceres::ColumnMajorAdapter3x3(rotationf.data()));\n results.push_back(rotationf);\n this->solve(incrementalConstraints,results);\n }\n }\n\n std::vector results2(num_rotations,Eigen::Matrix3d::Identity());\n this->solve(constraints,results2);\n \n double error0=0.0,error1=0.0,error2=0;\n \n for (int i=1;i& constraints,std::vector& results){\n \n //timeval time;\n //gettimeofday(&time, NULL);\n //long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);\n \n\n int variableCount=results.size();\n int constraintCount=constraints.size();\n int rowCount=2*constraintCount;\n \n int columnCount=variableCount+constraintCount;\n int singleVariableConstraintCount=0;\n \n for(int i=0;imaxIterations);\n model.loadProblem(byRow,columnLower,columnUpper,objective,rowLower,rowUpper);\n model.dual();\n const double *solutionPtr=model.primalColumnSolution();\n \n for (int i=0;i globalScales(num_scales);\n cv::RNG rng(-1);\n for (int i=0;i constraints;\n for (int i=0;inum_scales-100) {\n constraint.variableIndex1=-1;\n constraint.variableIndex2=i;\n constraint.value1=0.0;\n constraint.value12=std::log(globalScales[i]);\n constraint.weight=1.0;\n constraints.push_back(constraint);\n }\n \n if (i!=0) {\n continue;\n }\n constraint.variableIndex1=-1;\n constraint.variableIndex2=i;\n constraint.value1=0.0;\n constraint.value12=std::log(globalScales[i]);\n constraint.weight=1.0;\n constraints.push_back(constraint);\n \n\n }\n \n for (int i=0;i incrementalConstraints;\n std::vector results2(6,0);\n \n for (int f=5;fsolve(incrementalConstraints,results2);\n }else{\n std::vector v;\n for (int j=incrementalConstraints.size()-1;j>0;j--) {\n if (incrementalConstraints[j].variableIndex2==f) {\n v.push_back(incrementalConstraints[j].value12+results2[incrementalConstraints[j].variableIndex1]);\n }\n }\n std::sort(v.begin(),v.end());\n results2.push_back(v[v.size()/2]);\n this->maxIterations=1000;\n this->solve(incrementalConstraints,results2);\n }\n }\n \n this->maxIterations=10000;\n this->solve(incrementalConstraints,results2);\n \n this->maxIterations=10000;\n std::vector results(num_scales,0);\n this->solve(constraints,results);\n results2.resize(results.size());\n for (int i=1;i &constraints,std::vector& results){\n \n int variableCount=results.size();\n int constraintCount=constraints.size();\n int rowCount=2*constraintCount;\n \n int columnCount=variableCount+constraintCount;\n int singleVariableConstraintCount=0;\n \n for(int i=0;imaxIterations);\n \n for (int c=0;c<3;c++) {\n \n for (int i = 0; i &constraints,std::vector& results){\n \n }\n \n void GlobalTranslationEstimation::test(){\n \n int num_translation=500;\n std::vector globalTranslations(num_translation);\n std::vector globalRotations(num_translation);\n \n cv::RNG rng(-1);\n for (int i=0;i constraints;\n \n for (int i=0;i results(num_translation,Eigen::Vector3d::Zero());\n this->solve(constraints,results);\n \n double error0=0;\n double error1=0;\n for (int i=0;idata.db);\n \n int i;\n \n memset(sa, 0, sizeof(sa));\n memset(sb, 0, sizeof(sb));\n \n for (i = 0; i < count; i++)\n {\n sa[0] += a[i].x*a[i].x;\n sa[1] += a[i].y*a[i].x;\n sa[2] += a[i].x;\n \n sa[6] += a[i].x*a[i].y;\n sa[7] += a[i].y*a[i].y;\n sa[8] += a[i].y;\n \n sa[12] += a[i].x;\n sa[13] += a[i].y;\n sa[14] += 1;\n \n sb[0] += a[i].x*b[i].x;\n sb[1] += a[i].y*b[i].x;\n sb[2] += b[i].x;\n sb[3] += a[i].x*b[i].y;\n sb[4] += a[i].y*b[i].y;\n sb[5] += b[i].y;\n }\n \n sa[21] = sa[0];\n sa[22] = sa[1];\n sa[23] = sa[2];\n sa[27] = sa[6];\n sa[28] = sa[7];\n sa[29] = sa[8];\n sa[33] = sa[12];\n sa[34] = sa[13];\n sa[35] = sa[14];\n \n cvSolve(&A, &B, &MM, CV_SVD);\n }\n else\n {\n double sa[16], sb[4], m[4], *om = M->data.db;\n CvMat A = cvMat(4, 4, CV_64F, sa), B = cvMat(4, 1, CV_64F, sb);\n CvMat MM = cvMat(4, 1, CV_64F, m);\n \n int i;\n \n memset(sa, 0, sizeof(sa));\n memset(sb, 0, sizeof(sb));\n \n for (i = 0; i < count; i++)\n {\n sa[0] += a[i].x*a[i].x + a[i].y*a[i].y;\n sa[1] += 0;\n sa[2] += a[i].x;\n sa[3] += a[i].y;\n \n sa[4] += 0;\n sa[5] += a[i].x*a[i].x + a[i].y*a[i].y;\n sa[6] += -a[i].y;\n sa[7] += a[i].x;\n \n sa[8] += a[i].x;\n sa[9] += -a[i].y;\n sa[10] += 1;\n sa[11] += 0;\n \n sa[12] += a[i].y;\n sa[13] += a[i].x;\n sa[14] += 0;\n sa[15] += 1;\n \n sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;\n sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;\n sb[2] += b[i].x;\n sb[3] += b[i].y;\n }\n \n cvSolve(&A, &B, &MM, CV_SVD);\n \n om[0] = om[4] = m[0];\n om[1] = -m[1];\n om[3] = m[1];\n om[2] = m[2];\n om[5] = m[3];\n }\n }\n\n static int EstimateRigidTransform(cv::AutoBuffer &good_idx,\n int\t\t\t \t &good_count,\n const CvArr* matA, const CvArr* matB, CvMat* matM, int full_affine)\n {\n const int COUNT = 15;\n const int WIDTH = 160, HEIGHT = 120;\n const int RANSAC_MAX_ITERS = 500;\n const int RANSAC_SIZE0 = 3;\n const double RANSAC_GOOD_RATIO = 0.8;\n \n cv::Ptr sA, sB;\n cv::AutoBuffer pA, pB;\n cv::AutoBuffer status;\n cv::Ptr gray;\n \n CvMat stubA, *A = cvGetMat(matA, &stubA);\n CvMat stubB, *B = cvGetMat(matB, &stubB);\n CvSize sz0, sz1;\n int cn, equal_sizes;\n int i, j, k, k1;\n int count_x, count_y, count = 0;\n double scale = 1;\n CvRNG rng = cvRNG(-1);\n double m[6] = { 0 };\n CvMat M = cvMat(2, 3, CV_64F, m);\n good_count = 0;\n CvRect brect;\n \n if (!CV_IS_MAT(matM))\n CV_Error(matM ? CV_StsBadArg : CV_StsNullPtr, \"Output parameter M is not a valid matrix\");\n \n if (!CV_ARE_SIZES_EQ(A, B))\n CV_Error(CV_StsUnmatchedSizes, \"Both input images must have the same size\");\n \n if (!CV_ARE_TYPES_EQ(A, B))\n CV_Error(CV_StsUnmatchedFormats, \"Both input images must have the same data type\");\n \n if (CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2)\n {\n count = A->cols*A->rows;\n CvMat _pA, _pB;\n pA.allocate(count);\n pB.allocate(count);\n _pA = cvMat(A->rows, A->cols, CV_32FC2, pA);\n _pB = cvMat(B->rows, B->cols, CV_32FC2, pB);\n cvConvert(A, &_pA);\n cvConvert(B, &_pB);\n }\n else\n CV_Error(CV_StsUnsupportedFormat, \"Both input images must have either 8uC1 or 8uC3 type\");\n \n good_idx.allocate(count);\n \n if (count < RANSAC_SIZE0){\n //printf(\"too small!\\n\");\n return 0;\n }\n \n CvMat _pB = cvMat(1, count, CV_32FC2, pB);\n brect = cvBoundingRect(&_pB, 1);\n \n // RANSAC stuff:\n // 1. find the consensus\n for (k = 0; k < RANSAC_MAX_ITERS; k++)\n {\n //printf(\"ransac %d\\n\", k);\n int idx[RANSAC_SIZE0];\n CvPoint2D32f a[3];\n CvPoint2D32f b[3];\n \n memset(a, 0, sizeof(a));\n memset(b, 0, sizeof(b));\n \n // choose random 3 non-complanar points from A & B\n for (i = 0; i < RANSAC_SIZE0; i++)\n {\n for (k1 = 0; k1 < RANSAC_MAX_ITERS; k1++)\n {\n idx[i] = cvRandInt(&rng) % count;\n \n for (j = 0; j < i; j++)\n {\n if (idx[j] == idx[i])\n break;\n // check that the points are not very close one each other\n if (fabs(pA[idx[i]].x - pA[idx[j]].x) +\n fabs(pA[idx[i]].y - pA[idx[j]].y) < FLT_EPSILON)\n break;\n if (fabs(pB[idx[i]].x - pB[idx[j]].x) +\n fabs(pB[idx[i]].y - pB[idx[j]].y) < FLT_EPSILON)\n break;\n }\n \n if (j < i)\n continue;\n \n if (i + 1 == RANSAC_SIZE0)\n {\n // additional check for non-complanar vectors\n a[0] = pA[idx[0]];\n a[1] = pA[idx[1]];\n a[2] = pA[idx[2]];\n \n b[0] = pB[idx[0]];\n b[1] = pB[idx[1]];\n b[2] = pB[idx[2]];\n \n double dax1 = a[1].x - a[0].x, day1 = a[1].y - a[0].y;\n double dax2 = a[2].x - a[0].x, day2 = a[2].y - a[0].y;\n double dbx1 = b[1].x - b[0].x, dby1 = b[1].y - b[0].y;\n double dbx2 = b[2].x - b[0].x, dby2 = b[2].y - b[0].y;\n const double eps = 0.01;\n \n if (fabs(dax1*day2 - day1*dax2) < eps*sqrt(dax1*dax1 + day1*day1)*sqrt(dax2*dax2 + day2*day2) ||\n fabs(dbx1*dby2 - dby1*dbx2) < eps*sqrt(dbx1*dbx1 + dby1*dby1)*sqrt(dbx2*dbx2 + dby2*dby2))\n continue;\n }\n break;\n }\n \n if (k1 >= RANSAC_MAX_ITERS)\n break;\n }\n \n if (i < RANSAC_SIZE0)\n continue;\n \n // estimate the transformation using 3 points\n icvGetRTMatrix(a, b, 3, &M, full_affine);\n \n for (i = 0, good_count = 0; i < count; i++)\n {\n if (fabs(m[0] * pA[i].x + m[1] * pA[i].y + m[2] - pB[i].x) +\n fabs(m[3] * pA[i].x + m[4] * pA[i].y + m[5] - pB[i].y) < MAX(brect.width, brect.height)*0.005)\n good_idx[good_count++] = i;\n }\n \n if (good_count >= count*RANSAC_GOOD_RATIO)\n break;\n }\n \n if (k >= RANSAC_MAX_ITERS){\n //printf(\"ransac_max_iters\\n\");\n return 0;\n }\n \n if (good_count < count)\n {\n for (i = 0; i < good_count; i++)\n {\n j = good_idx[i];\n pA[i] = pA[j];\n pB[i] = pB[j];\n }\n }\n \n icvGetRTMatrix(pA, pB, good_count, &M, full_affine);\n m[2] /= scale;\n m[5] /= scale;\n cvConvert(&M, matM);\n \n return 1;\n }\n\n static cv::Mat EstimateRigidTransform(cv::AutoBuffer &goodIdx,\n int &good_count,\n cv::InputArray src1,\n cv::InputArray src2,\n bool fullAffine){\n cv::Mat M(2, 3, CV_64F), A = src1.getMat(), B = src2.getMat();\n CvMat matA = A, matB = B, matM = M;\n int err = EstimateRigidTransform(goodIdx, good_count, &matA, &matB, &matM, fullAffine);\n if (err == 1){\n return M;\n }else{\n M.setTo(0);\n M.at(0, 0) = 1.0;\n M.at(1, 1) = 1.0;\n return M;\n }\n }\n \n static bool homographyRefineLM(const std::vector& pts1,\n const std::vector& pts2,\n cv::Mat& homography,const int maxIters){\n \n CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));\n CvMat modelPart = cvMat(solver.param->rows, solver.param->cols,homography.type(),homography.ptr());\n cvCopy( &modelPart, solver.param );\n \n for(;;)\n {\n const CvMat* _param = 0;\n CvMat *_JtJ = 0, *_JtErr = 0;\n double* _errNorm = 0;\n \n if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))\n break;\n \n for(int i = 0; i data.db;\n double Mx = pts1[i].x, My = pts1[i].y;\n double ww = h[6]*Mx + h[7]*My + 1.;\n ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0;\n double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;\n double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;\n double err[] = { _xi - pts2[i].x, _yi - pts2[i].y };\n if( _JtJ || _JtErr )\n {\n double J[][8] =\n {\n { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },\n { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }\n };\n \n for(int j = 0; j < 8; j++ )\n {\n for(int k = j; k < 8; k++ )\n _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];\n _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];\n }\n }\n if( _errNorm )\n *_errNorm += err[0]*err[0] + err[1]*err[1];\n }\n }\n cvCopy(solver.param,&modelPart );\n return true;\n }\n}\n\nnamespace GSLAM{\nstatic cv::Mat EstimateHomography(\n const std::vector& pts1,\n const std::vector& pts2,\n std::vector& status,\n int& good_count){\n \n cv::AutoBuffer goodIdx;\n bool fullAffine=false;\n cv::Mat rigid=GSLAM::EstimateRigidTransform(goodIdx,good_count,pts1,pts2,fullAffine);\n \n cv::Mat homography=cv::Mat::eye(3,3,CV_64F);\n for (int r=0;r<2;r++) {\n for (int c=0;c<3;c++) {\n homography.at(r,c)=rigid.at(r,c);\n }\n }\n \n std::vector _pts1(good_count),_pts2(good_count);\n for (int i=0;i(pts1.size(),false);\n for (int i=0;i motions;\n std::vector gaps;\n \n \n void getDiffRotation(cv::Mat &rotation,\n const int startGyroIndex,\n const int endGyroIndex,\n const double startStamp,\n const double endStamp){\n \n double dt;\n if (startGyroIndex==endGyroIndex) {\n dt=endStamp-startStamp;\n rotation=rotation*getSkewRotation(motions[startGyroIndex].anglev * dt);\n }else{\n dt = motions[startGyroIndex+1].stamp-startStamp;\n rotation=rotation*getSkewRotation(motions[startGyroIndex].anglev * dt);\n \n for (int i=startGyroIndex+1;i0) {\n motionIndex--;\n }\n }\n \n \n void synchronization(std::vector &frameStamps,double ts){\n int frameCount = frameStamps.size();\n while (frameStamps[frameCount - 1] + ts>motions[motions.size()-1].stamp) {\n frameCount--;\n }\n frameStamps.resize(frameCount);\n }\n \n void getIntraFrameRotation(std::vector &rotations,\n const std::vector &originVertices,\n cv::Size imageSize,\n cv::Size sizeByVertex,\n int &globalIndex,\n double &globalStamp){\n \n //printf(\"%d %f\\n\",globalIndex,globalStamp);getchar();\n \n updateMotionIndex(globalStamp,globalIndex);\n rotations[0]=cv::Mat::eye(3,3,CV_64FC1);\n \n int preIndex=globalIndex;\n double preStamp=globalStamp;\n \n int rowIndex=globalIndex;\n double rowStamp=0;\n int rowVertexIndex=0;\n \n for (int i=1;i preparedGridVertices;\n std::vector warpedGridVertices;\n \n\n void initializeGridVertices(cv::Size gridSize,cv::Size sizeByVertex){\n \n originGridVertices.reserve(sizeByVertex.width*sizeByVertex.height);\n preparedGridVertices.resize(sizeByVertex.width*sizeByVertex.height);\n warpedGridVertices.resize(sizeByVertex.width*sizeByVertex.height);\n \n for (int i=0;i originGridVertices;\n cv::Size sizeByGrid;\n cv::Size sizeByVertex;\n cv::Size gridSize;\n \n const std::vector& getOriginGridVertices() const {return originGridVertices;};\n const std::vector& getPreparedGridVertices() const {return preparedGridVertices;};\n \n std::vector& getWarpedGridVertices() {return warpedGridVertices;};\n \n void initialize(cv::Size _gridSize,cv::Size _sizeByGrid,cv::Size _sizeByVertex){\n \n initializeGridVertices(_gridSize,_sizeByVertex);\n \n sizeByGrid=_sizeByGrid;\n sizeByVertex=_sizeByVertex;\n gridSize=_gridSize;\n \n }\n \n \n void rotateAndNormalizePoint(const cv::Point2f &src,\n cv::Mat &dst,\n const std::vector &rotations,\n const cv::Mat &invK){\n \n double T[9];\n cv::Mat transform(3,3,CV_64FC1,T);\n \n int h0=src.y/gridSize.height;\n int h1=h0+1;\n \n int vIndex0=h0*sizeByVertex.width;\n int vIndex1=vIndex0+sizeByVertex.width;\n \n double dist0=src.y-originGridVertices[vIndex0].y;\n double dist1=originGridVertices[vIndex1].y-src.y;\n double dist =dist0+dist1;\n \n dist0=dist1/dist;\n dist1=1-dist0;\n \n transform=dist0*rotations[h0]+dist1*rotations[h1];\n transform=transform.t()*invK;\n \n dst=cv::Mat(3,1,CV_64FC1);\n double *data=(double*)dst.data;\n data[0]= T[0]*src.x + T[1]*src.y + T[2];\n data[1]= T[3]*src.x + T[4]*src.y + T[5];\n data[2]= T[6]*src.x + T[7]*src.y + T[8];\n double normValue=cv::norm(dst,cv::NORM_L2);\n dst*=(1.0/normValue);\n \n return;\n }\n \n \n \n void prepareMeshByRow(const std::vector &transformArray){\n double T[9];\n cv::Mat transform(3,3,CV_64FC1,T);\n for (int h=0;h\n\n#pragma once\n\n#define USE_TBB\n#define SSE_TRACKING\n\ntypedef struct {\n \n float lighting_alpha;\n float lighting_beta;\n \n /* Available to user */\n int mindist;\t\t\t/* min distance b/w features */\n int window_width, window_height;\n bool sequentialMode;\t/* whether to save most recent image to save time */\n /* can set to TRUE manually, but don't set to */\n /* FALSE manually */\n bool smoothBeforeSelecting;\t/* whether to smooth image before */\n /* selecting features */\n bool writeInternalImages;\t/* whether to write internal images */\n /* tracking features */\n bool lighting_insensitive; /* whether to normalize for gain and bias (not in original algorithm) */\n \n /* Available, but hopefully can ignore */\n int min_eigenvalue;\t\t/* smallest eigenvalue allowed for selecting */\n float min_determinant;\t/* th for determining lost */\n float min_displacement;\t/* th for stopping tracking when pixel changes little */\n int max_iterations;\t\t/* th for stopping tracking when too many iterations */\n float max_residue;\t\t/* th for stopping tracking when residue is large */\n float grad_sigma;\n float smooth_sigma_fact;\n float pyramid_sigma_fact;\n float step_factor; /* size of Newton steps; 2.0 comes from equations, 1.0 seems to avoid overshooting */\n int nSkippedPixels;\t\t/* # of pixels skipped when finding features */\n int borderx;\t\t\t/* border in which features will not be found */\n int bordery;\n int nPyramidLevels;\t\t/* computed from search_ranges */\n int featureSelectionLevel;\n int subsampling;\t\t/* \t\t\" */\n \n \n /* for affine mapping */\n int affine_window_width, affine_window_height;\n int affineConsistencyCheck; /* whether to evaluates the consistency of features with affine mapping\n -1 = don't evaluates the consistency\n 0 = evaluates the consistency of features with translation mapping\n 1 = evaluates the consistency of features with similarity mapping\n 2 = evaluates the consistency of features with affine mapping\n */\n int affine_max_iterations;\n float affine_max_residue;\n float affine_min_displacement;\n float affine_max_displacement_differ; /* th for the difference between the displacement calculated\n by the affine tracker and the frame to frame tracker in pel*/\n \n} KLT_TrackingContextRec, *KLT_TrackingContext;\n\n#define KLT_TRACKED 0\n#define KLT_NOT_FOUND -1\n#define KLT_SMALL_DET -2\n#define KLT_MAX_ITERATIONS -3\n#define KLT_OOB -4\n#define KLT_LARGE_RESIDUE -5\n\ntypedef struct {\n \n float x;\n float y;\n int val;\n \n cv::Point2f pt;\n Eigen::Vector3d norm;\n Eigen::Vector3d vec;\n \n float aff_x;\n float aff_y;\n float aff_Axx;\n float aff_Ayx;\n float aff_Axy;\n float aff_Ayy;\n \n cv::Mat *aff_img;\n cv::Mat *aff_gradx;\n cv::Mat *aff_grady;\n cv::Mat coefficient;\n \n} KLT_FeatureRec,*KLT_Feature;\n\ntypedef struct {\n bool isOutlierRejected;\n int nFeatures;\n KLT_Feature *feature;\n} KLT_FeatureListRec, *KLT_FeatureList;\n\n/* Kernels */\n#define MAX_KERNEL_WIDTH 71\n\ntypedef struct {\n int width;\n float data[MAX_KERNEL_WIDTH];\n} ConvolutionKernel;\n\nstatic ConvolutionKernel gauss_kernel;\nstatic ConvolutionKernel gaussderiv_kernel;\nstatic float sigma_last = -10.0;\n\nstatic int KLT_verbose = 0;\ntypedef float KLT_locType;\ntypedef unsigned char KLT_PixelType;\n\n\n\n" }, { "path": "GSLAM/KLTUtil.cpp", "content": "//\n// KLT.cpp\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"KLTUtil.h\"\n#include \"error.h\"\n\nstatic const int _mindist = 10;\nstatic const int _window_size = 21;\nstatic const int _min_eigenvalue = 200;\nstatic const float _min_determinant = 0.01f;\nstatic const float _min_displacement = 0.1f;\nstatic const int _max_iterations = 10;\nstatic const float _max_residue = 10.0f;\n\n\nstatic const float _grad_sigma = 1.0f;\nstatic const float _smooth_sigma_fact = 0.1f;\nstatic const float _pyramid_sigma_fact = 0.9f;\n\n\n//static const float _grad_sigma = 0.5f;\n//static const float _smooth_sigma_fact = 0.1f;\n//static const float _pyramid_sigma_fact = 0.3f;\n\n\nstatic const float _step_factor = 1.0f;\nstatic const bool _sequentialMode = false;\nstatic const bool _lighting_insensitive = false;\n\n/* for affine mapping*/\nstatic const int _affineConsistencyCheck = -1;\nstatic const int _affine_window_size = 15;\nstatic const int _affine_max_iterations = 10;\nstatic const float _affine_max_residue = 10.0;\nstatic const float _affine_min_displacement = 0.02f;\nstatic const float _affine_max_displacement_differ = 1.5f;\n\nstatic const bool _smoothBeforeSelecting = true;\nstatic const bool _writeInternalImages = false;\nstatic const int _search_range = 15;\nstatic const int _nSkippedPixels = 0;\n\n\nKLT_FeatureList KLTCreateFeatureList(KLT_TrackingContext tc,int nFeatures)\n{\n KLT_FeatureList fl;\n KLT_Feature first;\n int nbytes = sizeof(KLT_FeatureListRec) +\n 100*nFeatures * sizeof(KLT_Feature) +\n 100*nFeatures * sizeof(KLT_FeatureRec);\n int i;\n \n /* Allocate memory for feature list */\n fl = (KLT_FeatureList)malloc(nbytes);\n \n /* Set parameters */\n fl->nFeatures = nFeatures;\n \n /* Set pointers */\n fl->feature = (KLT_Feature *) (fl + 1);\n first = (KLT_Feature) (fl->feature + 100*nFeatures);\n \n for (i = 0 ; i <100*nFeatures ; i++) {\n \n fl->feature[i] = first + i;\n \n \n fl->feature[i]->aff_x=-1.0;\n fl->feature[i]->aff_y=-1.0;\n \n fl->feature[i]->aff_img=NULL;\n fl->feature[i]->aff_gradx=NULL;\n fl->feature[i]->aff_grady=NULL;\n \n }\n /* Return feature list */\n return(fl);\n}\n\n\n\nvoid KLTChangeTCPyramid(KLT_TrackingContext tc,\n int search_range)\n{\n float window_halfwidth;\n float subsampling;\n \n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"(KLTChangeTCPyramid) Window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"(KLTChangeTCPyramid) Window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"(KLTChangeTCPyramid) Window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"(KLTChangeTCPyramid) Window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n window_halfwidth = std::min(tc->window_width,tc->window_height)/2.0f;\n \n subsampling = ((float) search_range) / window_halfwidth;\n \n if (subsampling < 1.0) {\t\t/* 1.0 = 0+1 */\n tc->nPyramidLevels = 1;\n } else if (subsampling <= 3.0) {\t/* 3.0 = 2+1 */\n tc->nPyramidLevels = 2;\n tc->subsampling = 2;\n } else if (subsampling <= 5.0) {\t/* 5.0 = 4+1 */\n tc->nPyramidLevels = 2;\n tc->subsampling = 4;\n } else if (subsampling <= 9.0) {\t/* 9.0 = 8+1 */\n tc->nPyramidLevels = 2;\n tc->subsampling = 8;\n } else {\n float val = (float) (log(7.0*subsampling+1.0)/log(8.0));\n tc->nPyramidLevels = (int) (val + 0.99);\n tc->subsampling = 8;\n }\n}\n\nvoid computeKernels(float sigma,\n ConvolutionKernel *gauss,\n ConvolutionKernel *gaussderiv)\n{\n const float factor = 0.01f; /* for truncating tail */\n int i;\n \n assert(MAX_KERNEL_WIDTH % 2 == 1);\n assert(sigma >= 0.0);\n \n /* Compute kernels, and automatically determine widths */\n {\n const int hw = MAX_KERNEL_WIDTH / 2;\n float max_gauss = 1.0f, max_gaussderiv = (float) (sigma*exp(-0.5f));\n \n /* Compute gauss and deriv */\n for (i = -hw ; i <= hw ; i++) {\n gauss->data[i+hw] = (float) exp(-i*i / (2*sigma*sigma));\n gaussderiv->data[i+hw] = -i * gauss->data[i+hw];\n }\n \n /* Compute widths */\n gauss->width = MAX_KERNEL_WIDTH;\n for (i = -hw ; fabs(gauss->data[i+hw] / max_gauss) < factor ;\n i++, gauss->width -= 2);\n gaussderiv->width = MAX_KERNEL_WIDTH;\n for (i = -hw ; fabs(gaussderiv->data[i+hw] / max_gaussderiv) < factor ;\n i++, gaussderiv->width -= 2);\n if (gauss->width == MAX_KERNEL_WIDTH ||\n gaussderiv->width == MAX_KERNEL_WIDTH)\n KLTError(\"(_computeKernels) MAX_KERNEL_WIDTH %d is too small for \"\n \"a sigma of %f\", MAX_KERNEL_WIDTH, sigma);\n }\n \n /* Shift if width less than MAX_KERNEL_WIDTH */\n \n for (i = 0 ; i < gauss->width ; i++)\n gauss->data[i] = gauss->data[i+(MAX_KERNEL_WIDTH-gauss->width)/2];\n \n for (i = 0 ; i < gaussderiv->width ; i++)\n gaussderiv->data[i] = gaussderiv->data[i+(MAX_KERNEL_WIDTH-gaussderiv->width)/2];\n \n /* Normalize gauss and deriv */\n \n {\n const int hw = gaussderiv->width / 2;\n float den;\n \n den = 0.0;\n for (i = 0 ; i < gauss->width ; i++)\n den += gauss->data[i];\n \n for (i = 0 ; i < gauss->width ; i++)\n gauss->data[i] /= den;\n \n den = 0.0;\n for (i = -hw ; i <= hw ; i++)\n den += i*gaussderiv->data[i+hw];\n \n for (i = -hw ; i <= hw ; i++)\n gaussderiv->data[i+hw] /= den;\n }\n sigma_last = sigma;\n}\n\n\n/*********************************************************************\n * _KLTGetKernelWidths\n *\n */\n\nvoid _KLTGetKernelWidths(float sigma,\n int *gauss_width,\n int *gaussderiv_width){\n computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel);\n *gauss_width = gauss_kernel.width;\n *gaussderiv_width = gaussderiv_kernel.width;\n}\n\nfloat _KLTComputeSmoothSigma(KLT_TrackingContext tc){\n return (tc->smooth_sigma_fact * std::max(tc->window_width, tc->window_height));\n}\n\nstatic float _pyramidSigma(KLT_TrackingContext tc){\n return (tc->pyramid_sigma_fact * tc->subsampling);\n}\n\n\n\nvoid KLTUpdateTCBorder(KLT_TrackingContext tc){\n float val;\n int pyramid_gauss_hw;\n int smooth_gauss_hw;\n int gauss_width, gaussderiv_width;\n int num_levels = tc->nPyramidLevels;\n int n_invalid_pixels;\n int window_hw;\n int ss = tc->subsampling;\n int ss_power;\n int border;\n int i;\n \n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"(KLTUpdateTCBorder) Window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"(KLTUpdateTCBorder) Window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"(KLTUpdateTCBorder) Window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"(KLTUpdateTCBorder) Window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n window_hw = std::max(tc->window_width, tc->window_height)/2;\n \n /* Find widths of convolution windows */\n _KLTGetKernelWidths(_KLTComputeSmoothSigma(tc),\n &gauss_width, &gaussderiv_width);\n \n smooth_gauss_hw = gauss_width/2;\n \n _KLTGetKernelWidths(_pyramidSigma(tc),\n &gauss_width, &gaussderiv_width);\n \n pyramid_gauss_hw = gauss_width/2;\n \n /* Compute the # of invalid pixels at each level of the pyramid.\n n_invalid_pixels is computed with respect to the ith level\n of the pyramid. So, e.g., if n_invalid_pixels = 5 after\n the first iteration, then there are 5 invalid pixels in\n level 1, which translated means 5*subsampling invalid pixels\n in the original level 0. */\n n_invalid_pixels = smooth_gauss_hw;\n for (i = 1 ; i < num_levels ; i++) {\n val = ((float) n_invalid_pixels + pyramid_gauss_hw) / ss;\n n_invalid_pixels = (int) (val + 0.99); /* Round up */\n }\n \n /* ss_power = ss^(num_levels-1) */\n ss_power = 1;\n for (i = 1 ; i < num_levels ; i++)\n ss_power *= ss;\n \n /* Compute border by translating invalid pixels back into */\n /* original image */\n border = (n_invalid_pixels + window_hw) * ss_power;\n \n tc->borderx = border;\n tc->bordery = border;\n}\n\n\nKLT_TrackingContext KLTCreateTrackingContext(){\n \n KLT_TrackingContext tc;\n \n /* Allocate memory */\n tc = (KLT_TrackingContext) malloc(sizeof(KLT_TrackingContextRec));\n \n /* Set values to default values */\n tc->mindist = _mindist;\n tc->window_width = _window_size;\n tc->window_height = _window_size;\n tc->sequentialMode = _sequentialMode;\n tc->smoothBeforeSelecting = _smoothBeforeSelecting;\n tc->writeInternalImages = _writeInternalImages;\n tc->lighting_insensitive = _lighting_insensitive;\n tc->min_eigenvalue = _min_eigenvalue;\n tc->min_determinant = _min_determinant;\n tc->max_iterations = _max_iterations;\n tc->min_displacement = _min_displacement;\n tc->max_residue = _max_residue;\n tc->grad_sigma = _grad_sigma;\n tc->smooth_sigma_fact = _smooth_sigma_fact;\n tc->pyramid_sigma_fact = _pyramid_sigma_fact;\n tc->step_factor = _step_factor;\n tc->nSkippedPixels = _nSkippedPixels;\n \n \n /* for affine mapping */\n tc->affineConsistencyCheck = _affineConsistencyCheck;\n tc->affine_window_width = _affine_window_size;\n tc->affine_window_height = _affine_window_size;\n tc->affine_max_iterations = _affine_max_iterations;\n tc->affine_max_residue = _affine_max_residue;\n tc->affine_min_displacement = _affine_min_displacement;\n tc->affine_max_displacement_differ = _affine_max_displacement_differ;\n \n /* Change nPyramidLevels and subsampling */\n KLTChangeTCPyramid(tc,_search_range);\n \n /* Update border, which is dependent upon */\n /* smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling */\n KLTUpdateTCBorder(tc);\n \n return(tc);\n}\n\n\n\n" }, { "path": "GSLAM/KLTUtil.h", "content": "//\n// KLTUtil.cpp\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"KLT.h\"\n#pragma once\n\nvoid computeKernels(float sigma,ConvolutionKernel *gauss,ConvolutionKernel *gaussderiv);\nfloat _KLTComputeSmoothSigma(KLT_TrackingContext tc);\n\nKLT_TrackingContext KLTCreateTrackingContext();\nKLT_FeatureList KLTCreateFeatureList(KLT_TrackingContext tc,int nFeatures);\n\n\n\n\n\n" }, { "path": "GSLAM/KeyFrame.cpp", "content": "//\n// KeyFrame.cpp\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"KeyFrame.h\"\n#include \"Estimation.h\"\n#include \"Geometry.h\"\n#include \"MapPoint.h\"\n//#include \"Drawer.h\"\n//#include \n//#include \"opencv2/viz.hpp\"\n\nnamespace GSLAM{\n \n long unsigned int KeyFrame::nNextId=0;\n\n KeyFrame::KeyFrame(Frame *F,KeyFrameDatabase *pKFDB,const Eigen::Matrix3d &invK,double _mScaleFactor):\n mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0),\n mBowVec(F->mBowVec), mFeatVec(F->mFeatVec),mpKeyFrameDB(pKFDB),\n mpORBvocabulary(F->mpORBvocabulary),mScaleFactor(_mScaleFactor){\n \n mnId=nNextId++;\n framePtr=F;\n F->keyFramePtr=this;\n \n //initialize for geometry\n mvRelativeEstimated.resize(framePtr->mvKeys.size());\n std::fill(mvRelativeEstimated.begin(),mvRelativeEstimated.end(),true);\n mvLocalFrames.clear();\n mvLocalMapPoints.resize(framePtr->mvKeys.size());\n \n for (int i=0;imvKeys[i].pt.x*mScaleFactor,\n (double)framePtr->mvKeys[i].pt.y*mScaleFactor,1.0);\n kpt=invK*kpt;\n \n //mvLocalMapPoints[i].pt=framePtr->mvKeys[i].pt*mScaleFactor;\n mvLocalMapPoints[i].norm=kpt;\n mvLocalMapPoints[i].norm.normalize();\n\n \n mvLocalMapPoints[i].vec=kpt;\n mvLocalMapPoints[i].vec/=mvLocalMapPoints[i].vec(2);\n \n mvLocalMapPoints[i].vecs.clear();\n mvLocalMapPoints[i].pVectors.clear();\n mvLocalMapPoints[i].isFullTrack=true;\n mvLocalMapPoints[i].isEstimated=false;\n mvLocalMapPoints[i].measurementCount=0;\n \n const int level = framePtr->mvKeysUn[i].octave;\n const int nLevels = framePtr->mnScaleLevels;\n \n mvLocalMapPoints[i].maxLevelScaleFactor=framePtr->mvScaleFactors[level];\n mvLocalMapPoints[i].minLevelScaleFactor=framePtr->mvScaleFactors[nLevels-1];\n#ifdef DEBUG\n //std::cout<mBowVec), mFeatVec(F->mFeatVec),mpKeyFrameDB(pKFDB),\n mpORBvocabulary(F->mpORBvocabulary){\n \n mnId=nNextId++;\n framePtr=F;\n F->keyFramePtr=this;\n \n //initialize for geometry\n mvRelativeEstimated.resize(featureList->nFeatures);\n std::fill(mvRelativeEstimated.begin(),mvRelativeEstimated.end(),true);\n mvLocalFrames.clear();\n mvLocalMapPoints.resize(featureList->nFeatures);\n \n for (int i=0;ifeature[i]->x,featureList->feature[i]->y,1.0);\n kpt=invK*kpt;\n \n mvLocalMapPoints[i].norm=kpt;\n mvLocalMapPoints[i].norm.normalize();\n \n \n mvLocalMapPoints[i].vec=kpt;\n mvLocalMapPoints[i].vec/=mvLocalMapPoints[i].vec(2);\n \n //mvLocalMapPoints[i].pt.x=featureList->feature[i]->x;\n //mvLocalMapPoints[i].pt.y=featureList->feature[i]->y;\n \n \n mvLocalMapPoints[i].vecs.clear();\n mvLocalMapPoints[i].pVectors.clear();\n mvLocalMapPoints[i].isFullTrack=true;\n mvLocalMapPoints[i].isEstimated=false;\n mvLocalMapPoints[i].measurementCount=0;\n \n#ifdef DEBUG\n //std::cout< KeyFrame::GetConnectedKeyFrames(){\n unique_lock lock(mMutexConnections);\n set s;\n for(map::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++)\n s.insert(mit->first);\n return s;\n }\n \n vector KeyFrame::GetBestCovisibilityKeyFrames(const int &N){\n \n unique_lock lock(mMutexConnections);\n \n if((int)mvpOrderedConnectedKeyFrames.size()(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);\n \n }\n \n \n\n \n int KeyFrame::appendRelativeEstimation(const int frameId,KeyFrame *keyFrame,\n const Eigen::Matrix3d &rotation,const Eigen::Vector3d &translation,\n const KLT_FeatureList featureList,const vector &pVectors){\n \n LocalFrame frame;\n frame.frameId=frameId;\n frame.measurementCount=0;\n frame.pose.rotation=rotation;\n frame.pose.translation=translation;\n \n int fullTrackCount=0;\n for (int i=0;ifeature[i]->val==KLT_TRACKED) {\n Eigen::Vector3d* vecPtr=new Eigen::Vector3d(featureList->feature[i]->vec);\n mvLocalMapPoints[i].vecs.push_back(vecPtr);\n mvLocalMapPoints[i].measurementCount++;\n frame.measurementCount++;\n }else{\n mvLocalMapPoints[i].vecs.push_back(static_cast(NULL));\n }\n }\n mvLocalFrames.push_back(frame);\n return fullTrackCount;\n }\n \n void KeyFrame::UpdateBestCovisibles(){\n vector > vPairs;\n vPairs.reserve(mConnectedKeyFrameWeights.size());\n for(map::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end();\n mit!=mend;\n mit++){\n vPairs.push_back(make_pair(mit->second,mit->first));\n }\n \n sort(vPairs.begin(),vPairs.end());\n list lKFs;\n list lWs;\n for(size_t i=0, iend=vPairs.size(); i(lKFs.begin(),lKFs.end());\n mvOrderedWeights = vector(lWs.begin(), lWs.end()); \n }\n \n int KeyFrame::appendKeyFrame(KeyFrame *keyFrame,Transform transform,vector& matches){\n \n int nInlierMatch=0;\n for (int i=0;i=0);\n }\n assert(!mConnectedKeyFrameWeights.count(keyFrame));\n printf(\"append %d to %d %d\\n\",this->frameId,keyFrame->frameId,nInlierMatch);\n if (nInlierMatch>=20) {\n if(!mConnectedKeyFrameWeights.count(keyFrame)){\n mConnectedKeyFrameWeights[keyFrame]=nInlierMatch;\n mConnectedKeyFramePoses[keyFrame]=transform;\n mConnectedKeyFrameMatches[keyFrame].resize(matches.size());\n std::copy(matches.begin(),matches.end(),mConnectedKeyFrameMatches[keyFrame].begin());\n }\n else if(mConnectedKeyFrameWeights[keyFrame]!=nInlierMatch){\n mConnectedKeyFrameWeights[keyFrame]=nInlierMatch;\n mConnectedKeyFramePoses[keyFrame]=transform;\n std::copy(matches.begin(),matches.end(),mConnectedKeyFrameMatches[keyFrame].begin());\n }\n else{\n return 0;\n }\n UpdateBestCovisibles();\n }else{\n assert(0);\n }\n //printf(\"%d append %d %d\\n\",frameId,keyFrame->frameId,nInlierMatch);\n //finally append frame\n \n /*for (int i=0;i=0) {\n printf(\"keyframe %d %d estimated matched keyframe %d %d estimated\\n\",\n frameId,i,mvLocalMapPoints[i].isEstimated,\n keyFrame->frameId,matches[i],keyFrame->mvLocalMapPoints[matches[i]].isEstimated);\n }\n }*/\n \n return nInlierMatch;\n }\n \n \n void KeyFrame::savePly(const char* savename){\n \n int num_point=0;\n for(int i=0;i::iterator mit=mConnectedKeyFramePoses.begin(),\n mend=mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n Transform pose=mit->second;\n if (pose.scale==-1.0) {\n of< (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n#ifndef KEYFRAME_H\n#define KEYFRAME_H\n\n\n#include \"./DBoW2/BowVector.h\"\n#include \"./DBoW2/FeatureVector.h\"\n#include \"ORBVocabulary.h\"\n#include \"Frame.h\"\n#include \"KLT.h\"\n#include \"MapPoint.h\"\n#include \"KeyFrameDatabase.h\"\n#include \n#include \n\nnamespace GSLAM{\n \n class KeyFrameDatabase;\n \n class LocalFrame{\n public:\n int measurementCount;\n int frameId;\n Transform pose;\n };\n \n class KeyFrame{\n \n public:\n \n std::thread baThread;\n \n KeyFrame(Frame *F,KeyFrameDatabase* pKFDB,const Eigen::Matrix3d &invK,double mScaleFactor);\n KeyFrame(Frame *F,KeyFrameDatabase* pKFDB,const Eigen::Matrix3d &invK,KLT_FeatureList featureList);\n \n KeyFrame* prevKeyFramePtr;\n KeyFrame* nextKeyFramePtr;\n \n Frame* framePtr;\n \n //Keyframe Index\n static long unsigned int nNextId;\n int mnId;\n int frameId;\n int outId;\n \n \n // MapPoints associated to keypoints\n KLT_FeatureList mFeatureList;\n std::vector mvLocalFrames;\n std::vector mvLocalMapPoints;\n std::vector mvRelativeEstimated;\n \n std::vector > trackedPoints;\n \n // BoW\n KeyFrameDatabase* mpKeyFrameDB;\n ORBVocabulary* mpORBvocabulary;\n DBoW2::BowVector mBowVec;\n DBoW2::FeatureVector mFeatVec;\n \n float mScaleFactor;\n Eigen::Matrix3d invK;\n Eigen::Matrix3d frameK;\n \n // Variables used by the keyframe database\n long unsigned int mnLoopQuery;\n int mnLoopWords;\n float mLoopScore;\n long unsigned int mnRelocQuery;\n int mnRelocWords;\n float mRelocScore;\n float minScore;\n \n \n // Covisibility\n std::set GetConnectedKeyFrames();\n std::vector GetBestCovisibilityKeyFrames(const int &N);\n \n std::map mConnectedKeyFramePoses;\n std::map > mConnectedKeyFrameMatches;\n std::map mConnectedKeyFrameWeights;\n \n std::vector mvpOrderedConnectedKeyFrames;\n std::vector mvOrderedWeights;\n \n // Global graph property\n Transform pose;\n double scale;\n double logScale;\n bool isGlobalFixed;\n \n // mutex\n std::mutex mMutexPose;\n std::mutex mMutexConnections;\n std::mutex mMutexFeatures;\n \n \n //return full track count\n int appendRelativeEstimation(const int frameId,KeyFrame* keyFrame,\n const Eigen::Matrix3d &rotation,const Eigen::Vector3d &translation,\n const KLT_FeatureList featureList,const vector &pVectors);\n \n int appendKeyFrame(KeyFrame* nextKeyFrame,Transform transform,vector& matches);\n \n void savePly(const char* name);\n void saveData2(const char* name);\n void visualize();\n \n private:\n void UpdateBestCovisibles();\n };\n}\n#endif\n" }, { "path": "GSLAM/KeyFrameConnection.cpp", "content": "//\n// KeyFrameConnection.cpp\n// GSLAM\n//\n// Created by ctang on 9/24/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"KeyFrameConnection.h\"\n#include \"Estimation.h\"\n\nnamespace GSLAM{\n \n void updateScore(ORBVocabulary* mpORBVocabulary,KeyFrame* keyFrame1,KeyFrame* keyFrame2){\n float score=mpORBVocabulary->score(keyFrame1->mBowVec,keyFrame2->mBowVec);\n if (scoreminScore) {\n keyFrame1->minScore=score;\n }\n if (scoreminScore) {\n keyFrame2->minScore=score;\n }\n }\n \n int KeyFrameConnection::connectLoop(KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n Transform& transform12,\n std::vector &matches12,\n std::vector &matches21){\n \n matches12.resize(keyFrame1->mvLocalMapPoints.size(),-1);\n matches21.resize(keyFrame2->mvLocalMapPoints.size(),-1);\n \n int nMatches12=matcherByBoW->SearchByBoW(keyFrame1,keyFrame2,matches12,matches21);\n printf(\"loop match0 %d %d %d\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches12);\n \n if (nMatches12>connectionThreshold) {\n \n PnPEstimator pnp;\n pnp.prob=0.99;\n pnp.threshold=0.01;\n \n nMatches12=pnp.estimate(keyFrame1,keyFrame2,matches12,matches21,transform12);\n printf(\"loop pnp %d %d %d\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches12);\n \n LocalBundleAdjustment localBA;\n localBA.projErrorThres=0.008;\n std::vector errors;\n \n localBA.BAIterations=5;\n nMatches12=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,\n transform12,matches12,matches21);\n printf(\"loop refine1 %d %d %d\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches12);\n \n nMatches12=matcherByProjection->SearchByProjection(keyFrame1,keyFrame2,transform12,matches12,matches21,3,64);\n nMatches12=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,transform12,\n matches12,matches21);\n \n printf(\"loop refine2 %d %d %d\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches12);\n \n transform12.scale=-1.0;\n }\n \n return nMatches12;\n }\n \n void KeyFrameConnection::connectLoop(KeyFrame* keyFrame2){\n \n vector vpCandidateKFs = keyFrameDatabase->DetectLoopCandidates(keyFrame2,keyFrame2->minScore*0.8);\n for (int i=0;iframeId,keyFrame2->frameId,keyFrame1->minScore,keyFrame2->minScore);\n \n assert(!keyFrame1->mConnectedKeyFramePoses.count(keyFrame2)\n &&!keyFrame2->mConnectedKeyFramePoses.count(keyFrame1));\n \n Transform transform12;\n std::vector matches12,matches21;\n int nMatches12=connectLoop(keyFrame1,keyFrame2,transform12,matches12,matches21);\n \n Transform transform21;\n std::vector _matches21,_matches12;\n int nMatches21=connectLoop(keyFrame2,keyFrame1,transform21,_matches21,_matches12);\n \n //printf(\"loop matched %d %d %d %d %f %f\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches12,nMatches21,transform12.scale,transform21.scale);\n //printf(\"thres %d\\n\",connectionThreshold);\n if (nMatches12>connectionThreshold||nMatches21>connectionThreshold) {\n \n LocalBundleAdjustment localBA;\n localBA.BAIterations=5;\n localBA.projErrorThres=0.08;\n \n\n for (int i=0;i=0&&_matches12[i]<0) {\n _matches21[matches12[i]]=i;\n //printf(\"%d %d added %d\\n\",keyFrame2->frameId,matches12[i],i);\n }\n }\n \n for (int i=0;i<_matches21.size();i++) {\n if (_matches21[i]>=0&&matches21[i]<0) {\n matches12[_matches21[i]]=i;\n //printf(\"%d %d added %d\\n\",keyFrame1->frameId,_matches21[i],i);\n }\n }\n \n nMatches12=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,transform12,matches12,matches21);\n nMatches21=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,transform21,_matches21,_matches12);\n //cout<frameId,keyFrame2->frameId,nMatches12,nMatches21);\n \n if(nMatches12>connectionThreshold/2&&nMatches21>connectionThreshold/2){\n keyFrame1->appendKeyFrame(keyFrame2,transform12,matches12);\n localBA.refineKeyFrameConnection(keyFrame1);\n updateScore(mpORBVocabulary,keyFrame1,keyFrame2);\n //printf(\"loop connected %d %d %d\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches12);\n //}\n \n //if (nMatches21>connectionThreshold/2&&nMatches12>connectionThreshold/2) {\n keyFrame2->appendKeyFrame(keyFrame1,transform21,_matches21);\n localBA.refineKeyFrameConnection(keyFrame2);\n updateScore(mpORBVocabulary,keyFrame1,keyFrame2);\n //printf(\"loop connected %d %d %d\\n\",keyFrame2->frameId,keyFrame1->frameId,nMatches21);\n \n /*for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),\n mend=keyFrame1->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n Transform transform32,transform23;\n \n std::vector matches32,matches23;\n std::vector _matches32,_matches23;\n \n int nMatches32=connectKeyFrame(mit->first,keyFrame1,keyFrame2,transform32,matches32,matches23);\n int nMatches23=connectKeyFrame(keyFrame2,keyFrame1,mit->first,transform23,_matches23,_matches32);\n \n if(nMatches32=0&&_matches32[i]<0) {\n _matches23[matches32[i]]=i;\n }\n }\n \n for (int i=0;i<_matches23.size();i++) {\n if (_matches23[i]>=0&&matches23[i]<0) {\n matches32[_matches23[i]]=i;\n }\n }\n \n localBA.BAIterations=5;\n nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23);\n nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32);\n \n if(nMatches32>connectionThreshold/2&&nMatches23>connectionThreshold/2){\n \n mit->first->appendKeyFrame(keyFrame2,transform32,matches32);\n localBA.refineKeyFrameConnection(mit->first);\n \n keyFrame2->appendKeyFrame(mit->first,transform23,_matches23);\n localBA.refineKeyFrameConnection(keyFrame2);\n \n updateScore(mpORBVocabulary,keyFrame2,mit->first);\n }\n }\n \n \n \n for(map::iterator mit=keyFrame2->mConnectedKeyFramePoses.begin(),\n mend=keyFrame2->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n KeyFrame* keyFrame3=mit->first;\n \n for(map::iterator mit2=keyFrame3->mConnectedKeyFramePoses.begin(),\n mend2=keyFrame3->mConnectedKeyFramePoses.end();\n mit2!=mend2;\n mit2++){\n \n if (keyFrame2->mConnectedKeyFramePoses.count(keyFrame3)) {\n continue;\n }\n \n Transform transform32,transform23;\n std::vector matches32,matches23;\n std::vector _matches32,_matches23;\n \n int nMatches32=connectKeyFrame(mit2->first,keyFrame3,keyFrame2,transform32,matches32,matches23);\n int nMatches23=connectKeyFrame(keyFrame2,keyFrame3,mit2->first,transform23,_matches23,_matches32);\n \n if(nMatches32=0&&_matches32[i]<0) {\n _matches23[matches32[i]]=i;\n }\n }\n \n for (int i=0;i<_matches23.size();i++) {\n if (_matches23[i]>=0&&matches23[i]<0) {\n matches32[_matches23[i]]=i;\n }\n }\n \n localBA.BAIterations=5;\n nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23);\n nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32);\n \n\n mit->first->appendKeyFrame(keyFrame2,transform32,matches32);\n localBA.refineKeyFrameConnection(mit->first);\n \n keyFrame2->appendKeyFrame(mit->first,transform23,_matches23);\n localBA.refineKeyFrameConnection(keyFrame2);\n updateScore(mpORBVocabulary,keyFrame2,mit->first);\n }\n }*/\n }\n }\n }\n }\n \n\n \n int KeyFrameConnection::connectKeyFrame(KeyFrame* keyFrame3,KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n Transform& transform32,std::vector &matches32,std::vector &matches23){\n \n if (keyFrame3==keyFrame2||keyFrame3->mConnectedKeyFramePoses.count(keyFrame2)) {\n return -1;\n }\n \n const Transform transform31=keyFrame3->mConnectedKeyFramePoses[keyFrame1];\n const Transform transform13=keyFrame1->mConnectedKeyFramePoses[keyFrame3];\n \n matches32.resize(keyFrame3->mvLocalMapPoints.size(),-1);\n matches23.resize(keyFrame2->mvLocalMapPoints.size(),-1);\n \n if (!keyFrame3->mConnectedKeyFrameMatches.count(keyFrame1)\n ||!keyFrame1->mConnectedKeyFrameMatches.count(keyFrame2)) {\n assert(0);\n return -1;\n }\n \n const std::vector& matches31=keyFrame3->mConnectedKeyFrameMatches[keyFrame1];\n const std::vector& matches12=keyFrame1->mConnectedKeyFrameMatches[keyFrame2];\n \n int nMatches32=0;\n for (int i=0;imvLocalMapPoints.size();i++) {\n int idx1=matches31[i];\n if (idx1<0) {\n continue;\n }\n //printf(\"%d %d %d %d %d %d\\n\",keyFrame3->frameId,keyFrame1->frameId,keyFrame2->frameId,i,idx1,matches12[idx1]);\n \n int index2=matches12[idx1];\n if (index2>=0) {\n assert(matches23[index2]==-1);\n matches32[i]=index2;\n matches23[index2]=i;\n nMatches32++;\n }\n }\n \n std::vector scales;\n for (int i=0;imvLocalMapPoints[matches31[i]].isEstimated){\n continue;\n }else if(!keyFrame3->mvLocalMapPoints[i].isEstimated){\n //printf(\"%d %d\\n\",keyFrame3->frameId,i);\n //std::cout<mvLocalMapPoints[i].measurementCount<mvLocalMapPoints[i].getPosition()-transform31.translation).norm();\n double distance1=keyFrame1->mvLocalMapPoints[matches31[i]].getPosition().norm();\n scales.push_back(distance3/distance1);\n \n distance3=keyFrame3->mvLocalMapPoints[i].getPosition().norm();\n distance1=(keyFrame1->mvLocalMapPoints[matches31[i]].getPosition()-transform13.translation).norm();\n scales.push_back(distance3/distance1);\n }\n \n double relativeScale;\n if (scales.size()>5) {\n std::sort(scales.begin(),scales.end());\n relativeScale=scales[scales.size()/2];\n }else{\n relativeScale=1.0;\n }\n \n \n \n Transform transform12=keyFrame1->mConnectedKeyFramePoses[keyFrame2];\n transform12.translation*=relativeScale;\n transform32=transform31.leftMultiply(transform12);\n \n LocalBundleAdjustment localBA;\n localBA.projErrorThres=0.008;\n localBA.BAIterations=5;\n printf(\"first pass\");\n nMatches32=localBA.refineKeyFrameMatches(keyFrame3,keyFrame2,transform32,matches32,matches23);\n nMatches32=matcherByProjection->SearchByProjection(keyFrame3,keyFrame2,transform32,matches32,matches23,3,64);\n printf(\"second pass\");\n nMatches32=localBA.refineKeyFrameMatches(keyFrame3,keyFrame2,transform32,matches32,matches23);\n \n return nMatches32;\n }\n \n void KeyFrameConnection::connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n std::vector &matches21){\n std::vector matches12;\n matches12.resize(keyFrame1->mvLocalMapPoints.size());\n matches21.resize(keyFrame2->mvLocalMapPoints.size());\n \n std::fill(matches12.begin(),matches12.end(),-1);\n std::fill(matches21.begin(),matches21.end(),-1);\n \n int nMatches1=matcherByTracking->SearchByTracking(keyFrame1,keyFrame2,matches12,matches21,80);\n int nMatches2=matcherByProjection->SearchByProjection(keyFrame1,keyFrame2,keyFrame1->mvLocalFrames.back().pose,\n matches12,matches21,3,64);\n \n LocalBundleAdjustment localBA;\n localBA.projErrorThres=0.008;\n Transform pose12=keyFrame1->mvLocalFrames.back().pose;\n localBA.BAIterations=5;\n int nMatches=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,pose12,matches12,matches21);\n }\n \n void KeyFrameConnection::connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2){\n \n std::vector matches12,matches21;\n matches12.resize(keyFrame1->mvLocalMapPoints.size());\n matches21.resize(keyFrame2->mvLocalMapPoints.size());\n \n std::fill(matches12.begin(),matches12.end(),-1);\n std::fill(matches21.begin(),matches21.end(),-1);\n \n int nMatches1=matcherByTracking->SearchByTracking(keyFrame1,keyFrame2,matches12,matches21,80);\n int nMatches2=matcherByProjection->SearchByProjection(keyFrame1,keyFrame2,keyFrame1->mvLocalFrames.back().pose,\n matches12,matches21,3,64);\n \n LocalBundleAdjustment localBA;\n localBA.projErrorThres=0.008;\n Transform pose12=keyFrame1->mvLocalFrames.back().pose;\n localBA.BAIterations=5;\n int nMatches=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,pose12,matches12,matches21);\n /*for (int i=0;i0&&matches12[j]>0){\n assert(matches12[i]!=matches12[j]);\n }\n }\n }*/\n std::vector _matches12,_matches21;\n _matches12.resize(keyFrame1->mvLocalMapPoints.size());\n _matches21.resize(keyFrame2->mvLocalMapPoints.size());\n \n std::fill(_matches12.begin(),_matches12.end(),-1);\n std::fill(_matches21.begin(),_matches21.end(),-1);\n \n std::vector scales;\n for (int i=0;i=0&&keyFrame2->mvLocalMapPoints[matches12[i]].isEstimated) {\n \n _matches12[i]=matches12[i];\n _matches21[_matches12[i]]=i;\n \n double distance2=keyFrame2->mvLocalMapPoints[matches12[i]].getPosition().norm();\n double distance1=(keyFrame1->mvLocalMapPoints[i].getPosition()\n -keyFrame1->mvLocalFrames.back().pose.translation).norm();\n scales.push_back(distance2/distance1);\n }\n }\n std::sort(scales.begin(),scales.end());\n double scale=scales[scales.size()/2];\n Transform pose21=pose12.inverse();\n pose21.translation*=scale;\n \n localBA.BAIterations=5;\n int _nMatches2=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,pose21,_matches21,_matches12);\n _nMatches2=matcherByProjection->SearchByProjection(keyFrame2,keyFrame1,pose21,_matches21,_matches12,3,64);\n localBA.BAIterations=5;\n _nMatches2=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,pose21,_matches21,_matches12);\n \n \n /*printf(\"%d %d nMatches %d %d\\n\",keyFrame1->frameId,keyFrame2->frameId,nMatches,_nMatches2);\n for (int i=0;i0&&matches12[j]>0){\n assert(matches12[i]!=matches12[j]);\n }\n }\n }\n \n for (int i=0;i<_matches21.size()-1;i++) {\n for (int j=i+1;j<_matches21.size();j++) {\n if(_matches21[i]>0&&_matches21[j]>0){\n assert(_matches21[i]!=_matches21[j]);\n }\n }\n }*/\n \n for (int i=0;i=0&&_matches12[i]<0) {\n _matches21[matches12[i]]=i;\n //printf(\"%d %d added %d\\n\",keyFrame2->frameId,matches12[i],i);\n }\n }\n \n for (int i=0;i<_matches21.size();i++) {\n if (_matches21[i]>=0&&matches21[i]<0) {\n matches12[_matches21[i]]=i;\n //printf(\"%d %d added %d\\n\",keyFrame1->frameId,_matches21[i],i);\n }\n }\n \n /*for (int i=0;i0&&matches12[j]>0){\n assert(matches12[i]!=matches12[j]);\n }\n }\n }\n \n for (int i=0;i<_matches21.size()-1;i++) {\n for (int j=i+1;j<_matches21.size();j++) {\n if(_matches21[i]>0&&_matches21[j]>0){\n //printf(\"%d %d %d\\n\",keyFrame2->frameId,_matches21[i],matches21[j]);\n assert(_matches21[i]!=_matches21[j]);\n }\n }\n }*/\n \n localBA.BAIterations=5;\n nMatches=localBA.refineKeyFrameMatches(keyFrame1,keyFrame2,pose12,matches12,matches21);\n _nMatches2=localBA.refineKeyFrameMatches(keyFrame2,keyFrame1,pose21,_matches21,_matches12);\n \n \n keyFrame1->appendKeyFrame(keyFrame2,keyFrame1->mvLocalFrames.back().pose,matches12);\n keyFrame1->nextKeyFramePtr=keyFrame2;\n \n keyFrame2->appendKeyFrame(keyFrame1,pose21,_matches21);\n keyFrame2->prevKeyFramePtr=keyFrame1;\n \n updateScore(mpORBVocabulary,keyFrame1,keyFrame2);\n \n localBA.projErrorThres=0.008;\n //second layer: connect keyframe2 to keyframe1's connection\n for(map::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),\n mend=keyFrame1->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n Transform transform32,transform23;\n \n std::vector matches32,matches23;\n std::vector _matches32,_matches23;\n \n int nMatches32=connectKeyFrame(mit->first,keyFrame1,keyFrame2,transform32,matches32,matches23);\n int nMatches23=connectKeyFrame(keyFrame2,keyFrame1,mit->first,transform23,_matches23,_matches32);\n \n if(nMatches32=0&&_matches32[i]<0) {\n _matches23[matches32[i]]=i;\n }\n }\n \n for (int i=0;i<_matches23.size();i++) {\n if (_matches23[i]>=0&&matches23[i]<0) {\n matches32[_matches23[i]]=i;\n }\n }\n \n localBA.BAIterations=5;\n nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23);\n nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32);\n \n if(nMatches32>connectionThreshold/2&&nMatches23>connectionThreshold/2){\n \n mit->first->appendKeyFrame(keyFrame2,transform32,matches32);\n localBA.refineKeyFrameConnection(mit->first);\n \n keyFrame2->appendKeyFrame(mit->first,transform23,_matches23);\n localBA.refineKeyFrameConnection(keyFrame2);\n \n updateScore(mpORBVocabulary,keyFrame2,mit->first);\n }\n }\n \n \n \n for(map::iterator mit=keyFrame2->mConnectedKeyFramePoses.begin(),\n mend=keyFrame2->mConnectedKeyFramePoses.end();\n mit!=mend;\n mit++){\n \n KeyFrame* keyFrame3=mit->first;\n \n for(map::iterator mit2=keyFrame3->mConnectedKeyFramePoses.begin(),\n mend2=keyFrame3->mConnectedKeyFramePoses.end();\n mit2!=mend2;\n mit2++){\n \n if (keyFrame2->mConnectedKeyFramePoses.count(keyFrame3)) {\n continue;\n }\n \n Transform transform32,transform23;\n std::vector matches32,matches23;\n std::vector _matches32,_matches23;\n \n int nMatches32=connectKeyFrame(mit2->first,keyFrame3,keyFrame2,transform32,matches32,matches23);\n int nMatches23=connectKeyFrame(keyFrame2,keyFrame3,mit2->first,transform23,_matches23,_matches32);\n \n if(nMatches32=0&&_matches32[i]<0) {\n _matches23[matches32[i]]=i;\n }\n }\n \n for (int i=0;i<_matches23.size();i++) {\n if (_matches23[i]>=0&&matches23[i]<0) {\n matches32[_matches23[i]]=i;\n }\n }\n \n localBA.BAIterations=5;\n nMatches32=localBA.refineKeyFrameMatches(mit->first,keyFrame2,transform32,matches32,matches23);\n nMatches23=localBA.refineKeyFrameMatches(keyFrame2,mit->first,transform23,_matches23,_matches32);\n \n \n mit->first->appendKeyFrame(keyFrame2,transform32,matches32);\n localBA.refineKeyFrameConnection(mit->first);\n \n keyFrame2->appendKeyFrame(mit->first,transform23,_matches23);\n localBA.refineKeyFrameConnection(keyFrame2);\n updateScore(mpORBVocabulary,keyFrame2,mit->first);\n }\n }\n \n connectionThreshold=80;\n connectLoop(keyFrame2);\n }\n}" }, { "path": "GSLAM/KeyFrameConnection.h", "content": "//\n// KeyFrameConnection.hpp\n// GSLAM\n//\n// Created by ctang on 9/24/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifndef KeyFrameConnection_hpp\n#define KeyFrameConnection_hpp\n\n#include \"KeyFrame.h\"\n#include \"ORBmatcher.h\"\n\nnamespace GSLAM {\n class KeyFrameConnection{\n public:\n \n ORBmatcher *matcherByBoW;\n ORBmatcher *matcherByTracking;\n ORBmatcher *matcherByProjection;\n \n ORBVocabulary* mpORBVocabulary;\n KeyFrameDatabase* keyFrameDatabase;\n \n void connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2);\n void connectKeyFrame(KeyFrame* keyFrame1,KeyFrame* keyFrame2,std::vector& matches12);\n int connectKeyFrame(KeyFrame* keyFrame3,KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n Transform& transform32,std::vector &matches32,std::vector &matches23);\n \n \n \n void connectLoop(KeyFrame* keyFrame2);\n int connectLoop(KeyFrame* keyFrame1,KeyFrame* keyFrame2,\n Transform& transform12,std::vector &matches12,std::vector &matches21);\n \n \n int connectionThreshold;\n };\n}\n#endif /* KeyFrameConnection_hpp */\n" }, { "path": "GSLAM/KeyFrameDatabase.cpp", "content": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n\n#include \"KeyFrameDatabase.h\"\n\n#include \"KeyFrame.h\"\n#include \"./DBoW2/BowVector.h\"\n\n#include\n\nusing namespace std;\n\nnamespace GSLAM{\n\nKeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc):\n mpVoc(&voc){\n mvInvertedFile.resize(voc.size());\n}\n\n\nvoid KeyFrameDatabase::add(KeyFrame *pKF){\n \n unique_lock lock(mMutex);\n\n for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)\n mvInvertedFile[vit->first].push_back(pKF);\n}\n\nvoid KeyFrameDatabase::erase(KeyFrame* pKF){\n \n unique_lock lock(mMutex);\n\n // Erase elements in the Inverse File for the entry\n for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)\n {\n // List of keyframes that share the word\n list &lKFs = mvInvertedFile[vit->first];\n\n for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)\n {\n if(pKF==*lit)\n {\n lKFs.erase(lit);\n break;\n }\n }\n }\n}\n\nvoid KeyFrameDatabase::clear(){\n \n mvInvertedFile.clear();\n mvInvertedFile.resize(mpVoc->size());\n}\n\n\nvector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore){\n \n set spConnectedKeyFrames = pKF->GetConnectedKeyFrames();\n list lKFsSharingWords;\n\n // Search all keyframes that share a word with current keyframes\n // Discard keyframes connected to the query keyframe\n {\n unique_lock lock(mMutex);\n\n for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)\n {\n list &lKFs = mvInvertedFile[vit->first];\n\n for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)\n {\n KeyFrame* pKFi=*lit;\n if(pKFi->mnLoopQuery!=pKF->mnId)\n {\n pKFi->mnLoopWords=0;\n if(!spConnectedKeyFrames.count(pKFi))\n {\n pKFi->mnLoopQuery=pKF->mnId;\n lKFsSharingWords.push_back(pKFi);\n }\n }\n pKFi->mnLoopWords++;\n }\n }\n }\n\n if(lKFsSharingWords.empty())\n return vector();\n\n list > lScoreAndMatch;\n\n // Only compare against those keyframes that share enough words\n int maxCommonWords=0;\n for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)\n {\n if((*lit)->mnLoopWords>maxCommonWords)\n maxCommonWords=(*lit)->mnLoopWords;\n }\n\n int minCommonWords = maxCommonWords*0.8f;\n\n int nscores=0;\n\n // Compute similarity score. Retain the matches whose score is higher than minScore\n for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)\n {\n KeyFrame* pKFi = *lit;\n\n if(pKFi->mnLoopWords>minCommonWords)\n {\n nscores++;\n\n float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);\n\n pKFi->mLoopScore = si;\n if(si>=minScore)\n lScoreAndMatch.push_back(make_pair(si,pKFi));\n }\n }\n\n if(lScoreAndMatch.empty())\n return vector();\n\n list > lAccScoreAndMatch;\n float bestAccScore = minScore;\n\n // Lets now accumulate score by covisibility\n for(list >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)\n {\n KeyFrame* pKFi = it->second;\n vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);\n\n float bestScore = it->first;\n float accScore = it->first;\n KeyFrame* pBestKF = pKFi;\n for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)\n {\n KeyFrame* pKF2 = *vit;\n if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)\n {\n accScore+=pKF2->mLoopScore;\n if(pKF2->mLoopScore>bestScore)\n {\n pBestKF=pKF2;\n bestScore = pKF2->mLoopScore;\n }\n }\n }\n\n lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));\n if(accScore>bestAccScore)\n bestAccScore=accScore;\n }\n\n // Return all those keyframes with a score higher than 0.75*bestScore\n float minScoreToRetain = 0.75f*bestAccScore;\n\n set spAlreadyAddedKF;\n vector vpLoopCandidates;\n vpLoopCandidates.reserve(lAccScoreAndMatch.size());\n\n for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)\n {\n if(it->first>minScoreToRetain)\n {\n KeyFrame* pKFi = it->second;\n if(!spAlreadyAddedKF.count(pKFi))\n {\n vpLoopCandidates.push_back(pKFi);\n spAlreadyAddedKF.insert(pKFi);\n }\n }\n }\n\n\n return vpLoopCandidates;\n}\n\nvector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F){\n \n list lKFsSharingWords;\n\n // Search all keyframes that share a word with current frame\n {\n unique_lock lock(mMutex);\n\n for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)\n {\n list &lKFs = mvInvertedFile[vit->first];\n\n for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)\n {\n KeyFrame* pKFi=*lit;\n if(pKFi->mnRelocQuery!=F->mnId)\n {\n pKFi->mnRelocWords=0;\n pKFi->mnRelocQuery=F->mnId;\n lKFsSharingWords.push_back(pKFi);\n }\n pKFi->mnRelocWords++;\n }\n }\n }\n if(lKFsSharingWords.empty())\n return vector();\n\n // Only compare against those keyframes that share enough words\n int maxCommonWords=0;\n for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)\n {\n if((*lit)->mnRelocWords>maxCommonWords)\n maxCommonWords=(*lit)->mnRelocWords;\n }\n\n int minCommonWords = maxCommonWords*0.8f;\n\n list > lScoreAndMatch;\n\n int nscores=0;\n\n // Compute similarity score.\n for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)\n {\n KeyFrame* pKFi = *lit;\n\n if(pKFi->mnRelocWords>minCommonWords)\n {\n nscores++;\n float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);\n pKFi->mRelocScore=si;\n lScoreAndMatch.push_back(make_pair(si,pKFi));\n }\n }\n\n if(lScoreAndMatch.empty())\n return vector();\n\n list > lAccScoreAndMatch;\n float bestAccScore = 0;\n\n // Lets now accumulate score by covisibility\n for(list >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)\n {\n \n KeyFrame* pKFi = it->second;\n vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);\n\n float bestScore = it->first;\n float accScore = bestScore;\n KeyFrame* pBestKF = pKFi;\n for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)\n {\n KeyFrame* pKF2 = *vit;\n if(pKF2->mnRelocQuery!=F->mnId)\n continue;\n\n accScore+=pKF2->mRelocScore;\n if(pKF2->mRelocScore>bestScore)\n {\n pBestKF=pKF2;\n bestScore = pKF2->mRelocScore;\n }\n\n }\n lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));\n if(accScore>bestAccScore)\n bestAccScore=accScore;\n }\n\n // Return all those keyframes with a score higher than 0.75*bestScore\n float minScoreToRetain = 0.75f*bestAccScore;\n set spAlreadyAddedKF;\n vector vpRelocCandidates;\n vpRelocCandidates.reserve(lAccScoreAndMatch.size());\n for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)\n {\n const float &si = it->first;\n if(si>minScoreToRetain)\n {\n KeyFrame* pKFi = it->second;\n if(!spAlreadyAddedKF.count(pKFi))\n {\n vpRelocCandidates.push_back(pKFi);\n spAlreadyAddedKF.insert(pKFi);\n }\n }\n }\n\n return vpRelocCandidates;\n}\n\n}\n" }, { "path": "GSLAM/KeyFrameDatabase.h", "content": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n\n#ifndef KEYFRAMEDATABASE_H\n#define KEYFRAMEDATABASE_H\n\n#include \n#include \n#include \n\n#include \"KeyFrame.h\"\n#include \"Frame.h\"\n#include \"ORBVocabulary.h\"\n\n#include\n\n\nnamespace GSLAM\n{\n\nclass KeyFrame;\nclass Frame;\n\n\nclass KeyFrameDatabase\n{\npublic:\n\n KeyFrameDatabase(const ORBVocabulary &voc);\n\n void add(KeyFrame* pKF);\n\n void erase(KeyFrame* pKF);\n\n void clear();\n\n // Loop Detection\n std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore);\n \n // Relocalization\n std::vector DetectRelocalizationCandidates(Frame* F);\n\nprotected:\n\n // Associated vocabulary\n const ORBVocabulary* mpVoc;\n \n // Inverted file\n std::vector > mvInvertedFile;\n \n // Mutex\n std::mutex mMutex;\n};\n\n} //namespace ORB_SLAM\n\n#endif\n" }, { "path": "GSLAM/L1Solver.h", "content": "//\n// L1Solver.h\n// GSLAM\n//\n// Created by ctang on 10/2/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifndef L1Solver_h\n#define L1Solver_h\n\n#include \"./coin/ClpSimplex.hpp\"\n#include \"./coin/ClpPresolve.hpp\"\n#include \"./coin/ClpPrimalColumnSteepest.hpp\"\n#include \"./coin/ClpDualRowSteepest.hpp\"\n\n#endif /* L1Solver_h */\n" }, { "path": "GSLAM/LK.hpp", "content": "//\n// trackFeatures.hpp\n// STARCK\n//\n// Created by Chaos on 4/1/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n#include \"KLT.h\"\n#include \"ImageGrids.hpp\"\n#include \"error.h\"\n#include \n#include \n#include \"tbb/tbb.h\"\n#include \"Homography.hpp\"\n#include \n#include \"opencv2/imgproc/imgproc.hpp\"\n\nconst bool compensate_motion=false;\nconst bool compensate_lighting=false;\n\nstatic float lighting_alpha;\nstatic float lighting_beta;\ncv::Mat homography;\n\n\nint KLTCountRemainingFeatures(KLT_FeatureList fl){\n \n int count = 0;\n int i;\n \n for (i = 0 ; i < fl->nFeatures ; i++){\n if (fl->feature[i]->val >= 0){\n count++;\n }\n }\n return count;\n}\n\nconst int simd_step=8;\n\n\nstatic void compensatePatchLighting(float* patch,\n const int width,const int height,\n const int alignedPatchSize,const float beta){\n \n __m256 _beta = _mm256_set1_ps(beta);\n for(int p=0;p(yt+h,xt-hw),width*sizeof(float));\n }\n return;\n }\n \n __m256 weight00 = _mm256_set1_ps(weights[0]);\n __m256 weight01 = _mm256_set1_ps(weights[1]);\n __m256 weight10 = _mm256_set1_ps(weights[2]);\n __m256 weight11 = _mm256_set1_ps(weights[3]);\n \n for (int h=-hh,y=0;h<=hh;h++,y++) {\n float *imagePtr0=(float*)image.ptr(h+yt);\n float *imagePtr1=(float*)image.ptr(h+yt+1);\n \n for (int w=-hw,x=0;w<=hw;w+=simd_step,x+=simd_step) {\n \n __m256 patch00=_mm256_mul_ps(_mm256_load_ps(&imagePtr0[xt+w]),weight00);\n __m256 patch01=_mm256_mul_ps(_mm256_load_ps(&imagePtr0[xt+w+1]),weight01);\n __m256 patch10=_mm256_mul_ps(_mm256_load_ps(&imagePtr1[xt+w]),weight10);\n __m256 patch11=_mm256_mul_ps(_mm256_load_ps(&imagePtr1[xt+w+1]),weight11);\n \n __m256 newpatch=_mm256_add_ps(_mm256_add_ps(patch00,patch01),_mm256_add_ps(patch10,patch11));\n _mm256_store_ps(&patch[y*width+x],newpatch);\n }\n }\n memset(&patch[width*height],0,(alignedPatchSize-width*height)*sizeof(float));\n}\n\nstatic void computeImageDiff(const float* patch1,const float* patch2,float* diff,const int patchsize){\n for (int i=0;i ncols-1-borderx ||\n y < bordery || y > nrows-1-bordery );\n}\n\nstatic float computeABSImageDiff(float *patchdiff,const int patchsize){\n \n __m256 a = _mm256_set1_ps(-0.0);\n __m256 absSum=_mm256_setzero_ps();\n \n for (int i=0;idata + (img->ncols*yt) + xt;\n\n assert (xt >= 0 && yt >= 0 && xt <= img->ncols - 2 && yt <= img->nrows - 2);\n //printf(\"coef %f %f\\n\",ax,ay);\n return ( (1-ax) * (1-ay) * *ptr +\n ax * (1-ay) * *(ptr+1) +\n (1-ax) * ay * *(ptr+(img->ncols)) +\n ax * ay * *(ptr+(img->ncols)+1) );\n}\n\n\n/*********************************************************************\n * _computeIntensityDifference\n *\n * Given two images and the window center in both images,\n * aligns the images wrt the window and computes the difference\n * between the two overlaid images.\n */\n\nstatic void _computeIntensityDifference(\n _KLT_FloatImage img1, /* images */\n _KLT_FloatImage img2,\n float x1, float y1, /* center of window in 1st img */\n float x2, float y2, /* center of window in 2nd img */\n int width, int height, /* size of window */\n _FloatWindow imgdiff) /* output */\n{\n \n register int hw = width/2, hh = height/2;\n float g1, g2;\n register int i, j;\n \n /* Compute values */\n for (j = -hh ; j <= hh ; j++)\n for (i = -hw ; i <= hw ; i++) {\n g1 = _interpolate(x1+i, y1+j, img1);\n g2 = _interpolate(x2+i, y2+j, img2);\n *imgdiff++ = g1 - g2;\n }\n}\n\n\n/*********************************************************************\n * _computeGradientSum\n *\n * Given two gradients and the window center in both images,\n * aligns the gradients wrt the window and computes the sum of the two\n * overlaid gradients.\n */\n\nstatic void _computeGradientSum(\n _KLT_FloatImage gradx1, /* gradient images */\n _KLT_FloatImage grady1,\n _KLT_FloatImage gradx2,\n _KLT_FloatImage grady2,\n float x1, float y1, /* center of window in 1st img */\n float x2, float y2, /* center of window in 2nd img */\n int width, int height, /* size of window */\n _FloatWindow gradx, /* output */\n _FloatWindow grady) /* \" */\n{\n register int hw = width/2, hh = height/2;\n float g1, g2;\n register int i, j;\n \n /* Compute values */\n for (j = -hh ; j <= hh ; j++)\n for (i = -hw ; i <= hw ; i++) {\n g1 = _interpolate(x1+i, y1+j, gradx1);\n g2 = _interpolate(x2+i, y2+j, gradx2);\n *gradx++ = g1 + g2;\n g1 = _interpolate(x1+i, y1+j, grady1);\n g2 = _interpolate(x2+i, y2+j, grady2);\n *grady++ = g1 + g2;\n }\n}\n\n\n/*********************************************************************\n * _compute2by2GradientMatrix\n *\n */\n\nstatic void _compute2by2GradientMatrix(\n _FloatWindow gradx,\n _FloatWindow grady,\n int width, /* size of window */\n int height,\n float *gxx, /* return values */\n float *gxy,\n float *gyy)\n\n{\n register float gx, gy;\n register int i;\n \n /* Compute values */\n *gxx = 0.0; *gxy = 0.0; *gyy = 0.0;\n for (i = 0 ; i < width * height ; i++) {\n gx = *gradx++;\n gy = *grady++;\n *gxx += gx*gx;\n *gxy += gx*gy;\n *gyy += gy*gy;\n }\n}\n\n\n/*********************************************************************\n * _compute2by1ErrorVector\n *\n */\n\nstatic void _compute2by1ErrorVector(\n _FloatWindow imgdiff,\n _FloatWindow gradx,\n _FloatWindow grady,\n int width, /* size of window */\n int height,\n float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */\n float *ex, /* return values */\n float *ey)\n{\n register float diff;\n register int i;\n \n /* Compute values */\n *ex = 0; *ey = 0; \n for (i = 0 ; i < width * height ; i++) {\n diff = *imgdiff++;\n *ex += diff * (*gradx++);\n *ey += diff * (*grady++);\n }\n *ex *= step_factor;\n *ey *= step_factor;\n}\n\n\n\n\n/*********************************************************************\n * _allocateFloatWindow\n */\n\nstatic _FloatWindow _allocateFloatWindow(\n int width,\n int height)\n{\n _FloatWindow fw;\n \n fw = (_FloatWindow) malloc(width*height*sizeof(float));\n if (fw == NULL) KLTError(\"(_allocateFloatWindow) Out of memory.\");\n return fw;\n}\n\n\n\nstatic float _sumAbsFloatWindow(\n _FloatWindow fw,\n int width,\n int height)\n{\n float sum = 0.0;\n int w;\n \n for ( ; height > 0 ; height--)\n for (w=0 ; w < width ; w++)\n sum += (float) fabs(*fw++);\n \n return sum;\n}\n\n\n\n\n\n\n\nstatic int _trackFeature(float x1, /* location of window in first image */\n float y1,\n float *x2, /* starting location of search in second image */\n float *y2,\n cv::Mat &img1,\n cv::Mat &gradx1,\n cv::Mat &grady1,\n cv::Mat &img2,\n cv::Mat &gradx2,\n cv::Mat &grady2,\n int width, /* size of window */\n int height,\n float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */\n int max_iterations,\n float small, /* determinant threshold for declaring KLT_SMALL_DET */\n float th, /* displacement threshold for stopping */\n float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */\n int lighting_insensitive) /* whether to normalize for gain and bias */\n{\n \n float gxx, gxy, gyy, ex, ey, dx, dy;\n int iteration = 0;\n int status;\n int hw = width/2;\n int hh = height/2;\n int nc = img1.cols;\n int nr = img1.rows;\n float one_plus_eps = 1.001f; /* To prevent rounding errors */\n \n float weights1[4],weights2[4];\n \n \n \n#ifdef SSE_TRACKING\n \n int alignedPatchSize=simd_step*((width*height+simd_step)/simd_step);\n \n \n float *buffers=(float*)malloc(9*alignedPatchSize*sizeof(float));\n memset(buffers,0,9*alignedPatchSize*sizeof(float));\n \n float *patch1=buffers;\n float *gx1=&patch1[alignedPatchSize];\n float *gy1=&gx1[alignedPatchSize];\n \n float *patch2=&gy1[alignedPatchSize];\n float *gx2=&patch2[alignedPatchSize];\n float *gy2=&gx2[alignedPatchSize];\n \n float *patchdiff=&gy2[alignedPatchSize];\n float *gx=&patchdiff[alignedPatchSize];\n float *gy=&gx[alignedPatchSize];\n \n\n int xt1,yt1;\n computeBilinearWeight(x1,y1,xt1,yt1,weights1);\n \n if(compensate_lighting){\n for(int i=0;i<4;i++)\n weights1[i]*=lighting_alpha;\n }\n computeBilinearPatch(patch1,img1,xt1,yt1,width,height,alignedPatchSize,weights1);\n computeBilinearPatch(gx1,gradx1,xt1,yt1,width,height,alignedPatchSize,weights1);\n computeBilinearPatch(gy1,grady1,xt1,yt1,width,height,alignedPatchSize,weights1);\n \n if(compensate_lighting){\n //printf(\"aligned size %d\\n\",alignedPatchSize);\n /*float _patch[alignedPatchSize];\n memset(&_patch[width*height],0,(alignedPatchSize-width*height)*sizeof(float));\n for(int i=0;idata=(float*)img1.data;\n kltimage1->ncols=img1.cols;\n kltimage1->nrows=img1.rows;\n \n kltimage2->data=(float*)img2.data;\n kltimage2->ncols=img2.cols;\n kltimage2->nrows=img2.rows;\n \n kltgradx1->data=(float*)gradx1.data;\n kltgradx1->ncols=gradx1.cols;\n kltgradx1->nrows=gradx1.rows;\n \n \n kltgradx2->data=(float*)gradx2.data;\n kltgradx2->ncols=gradx2.cols;\n kltgradx2->nrows=gradx2.rows;\n \n kltgrady1->data=(float*)grady1.data;\n kltgrady1->ncols=grady1.cols;\n kltgrady1->nrows=grady1.rows;\n\n \n kltgrady2->data=(float*)grady2.data;\n kltgrady2->ncols=grady2.cols;\n kltgrady2->nrows=grady2.rows;\n#endif\n \n\n //printf(\"%f %f\\n\",x1,y1);\n /* Iteratively update the window position */\n do {\n \n /* If out of bounds, exit loop */\n if ( x1-hw < 0.0f || nc-( x1+hw) < one_plus_eps ||\n *x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps ||\n y1-hh < 0.0f || nr-( y1+hh) < one_plus_eps ||\n *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) {\n status = KLT_OOB;\n break;\n }\n \n#ifdef SSE_TRACKING\n \n int xt2,yt2;\n computeBilinearWeight(*x2,*y2,xt2,yt2,weights2);\n computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2);\n computeBilinearPatch(gx2,gradx2,xt2,yt2,width,height,alignedPatchSize,weights2);\n computeBilinearPatch(gy2,grady2,xt2,yt2,width,height,alignedPatchSize,weights2);\n\n \n computeImageDiff(patch1,patch2,patchdiff,width*height);\n computeImageSum(gx1,gx2,gx,width*height);\n computeImageSum(gy1,gy2,gy,width*height);\n \n\n compute2X2GradientMatrix(gx,gy,width*height,gxx,gyy,gxy);\n compute2X1ErrorVector(patchdiff,gx,gy,alignedPatchSize,step_factor,ex,ey);\n#else\n \n \n _computeIntensityDifference(kltimage1,kltimage2, x1, y1, *x2, *y2,\n width, height, imgdiff);\n \n \n \n \n \n _computeGradientSum(kltgradx1,kltgrady1,kltgradx2,kltgrady2,\n x1, y1, *x2, *y2, width, height, gradx, grady);\n \n \n _compute2by2GradientMatrix(gradx, grady, width, height,\n &gxx, &gxy, &gyy);\n \n _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor,\n &ex, &ey);\n#endif\n \n /* Using matrices, solve equation for new displacement */\n status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy);\n \n if (status == KLT_SMALL_DET){\n break;\n }\n \n *x2 += dx;\n *y2 += dy;\n iteration++;\n //printf(\"%f %f\\n\",dx,dy);\n \n } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations);\n //printf(\"max iter %d\\n\",max_iterations);\n //getchar();\n \n /* Check whether window is out of bounds */\n if (*x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps ||\n *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps){\n status = KLT_OOB;\n }\n \n /* Check whether residue is too large */\n if (status == KLT_TRACKED) {\n \n#ifdef SSE_TRACKING\n int xt2,yt2;\n computeBilinearWeight(*x2,*y2,xt2,yt2,weights2);\n computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2);\n computeImageDiff(patch1,patch2,patchdiff,alignedPatchSize);\n \n if (computeABSImageDiff(patchdiff,alignedPatchSize)/(width*height)> max_residue){\n status = KLT_LARGE_RESIDUE;\n }\n#else\n \n _computeIntensityDifference(kltimage1,kltimage2, x1, y1, *x2, *y2,\n width, height, imgdiff);\n float diff2=_sumAbsFloatWindow(imgdiff, width, height)/(width*height);\n if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue)\n status = KLT_LARGE_RESIDUE;\n#endif\n \n }\n#ifdef SSE_TRACKING\n free(buffers);\n#else\n free(imgdiff); free(gradx); free(grady);\n#endif\n \n \n /* Return appropriate value */\n if (status == KLT_SMALL_DET){\n return KLT_SMALL_DET;\n }\n else if (status == KLT_OOB){\n return KLT_OOB;\n }\n else if (status == KLT_LARGE_RESIDUE){\n return KLT_LARGE_RESIDUE;\n }\n else if (iteration >= max_iterations) {\n return KLT_MAX_ITERATIONS;\n }\n else return KLT_TRACKED;\n \n}\n\nstatic int refineFeatureTranslation(float *x2,\n float *y2,\n cv::Mat img1,\n cv::Mat gradx1,\n cv::Mat grady1,\n cv::Mat img2,\n cv::Mat gradx2,\n cv::Mat grady2,\n int width, /* size of window */\n int height,\n float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */\n int max_iterations,\n float small, /* determinant threshold for declaring KLT_SMALL_DET */\n float th,\n float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */\n float mdd) /* used affine mapping */\n{\n \n float gxx, gxy, gyy, ex, ey, dx, dy;\n int iteration = 0;\n int status = 0;\n int hw = width/2;\n int hh = height/2;\n \n int nc2 = img2.cols;\n int nr2 = img2.rows;\n \n\n float one_plus_eps = 1.001f; /* To prevent rounding errors */\n float old_x2 = *x2;\n float old_y2 = *y2;\n \n int alignedPatchSize=simd_step*((width*height+simd_step)/simd_step);\n \n \n float *buffers=(float*)malloc(9*alignedPatchSize*sizeof(float));\n memset(buffers,0,9*alignedPatchSize*sizeof(float));\n \n float *patch1=buffers;\n float *gx1=&patch1[alignedPatchSize];\n float *gy1=&gx1[alignedPatchSize];\n \n float *patch2=&gy1[alignedPatchSize];\n float *gx2=&patch2[alignedPatchSize];\n float *gy2=&gx2[alignedPatchSize];\n \n float *patchdiff=&gy2[alignedPatchSize];\n float *gx=&patchdiff[alignedPatchSize];\n float *gy=&gx[alignedPatchSize];\n \n memcpy(patch1,img1.data,img1.cols*img1.rows*sizeof(float));\n memcpy(gx1,gradx1.data,gradx1.cols*gradx1.rows*sizeof(float));\n memcpy(gy1,grady1.data,grady1.cols*grady1.rows*sizeof(float));\n \n \n float weights2[4];\n /* Iteratively update the window position */\n do {\n \n if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps ||\n *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) {\n status = KLT_OOB;\n break;\n }\n \n int xt2,yt2;\n computeBilinearWeight(*x2,*y2,xt2,yt2,weights2);\n \n computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2);\n computeBilinearPatch(gx2,gradx2,xt2,yt2,width,height,alignedPatchSize,weights2);\n computeBilinearPatch(gy2,grady2,xt2,yt2,width,height,alignedPatchSize,weights2);\n \n \n computeImageDiff(patch1,patch2,patchdiff,width*height);\n computeImageSum(gx1,gx2,gx,width*height);\n computeImageSum(gy1,gy2,gy,width*height);\n \n compute2X2GradientMatrix(gx,gy,alignedPatchSize,gxx,gyy,gxy);\n \n compute2X1ErrorVector(patchdiff,gx,gy,alignedPatchSize,step_factor,ex,ey);\n \n status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy);\n \n if (status == KLT_SMALL_DET){\n break;\n }\n \n *x2 += dx;\n *y2 += dy;\n iteration++;\n \n \n } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations);\n \n \n /* Check whether window is out of bounds */\n if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps ||\n *y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps)\n status = KLT_OOB;\n \n /* Check whether feature point has moved to much during iteration*/\n if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd )\n status = KLT_OOB;\n \n \n /* Check whether residue is too large */\n if (status == KLT_TRACKED) {\n \n int xt2,yt2;\n computeBilinearWeight(*x2,*y2,xt2,yt2,weights2);\n computeBilinearPatch(patch2,img2,xt2,yt2,width,height,alignedPatchSize,weights2);\n computeImageDiff(patch1,patch2,patchdiff,alignedPatchSize);\n \n //printf(\"residual %f\\n\",computeABSImageDiff(patchdiff,alignedPatchSize)/(width*height));\n if (computeABSImageDiff(patchdiff,alignedPatchSize)/(width*height)> max_residue){\n status = KLT_LARGE_RESIDUE;\n }\n }\n \n free(buffers);\n return status;\n}\n\n\n\n\n\nclass KLTInvoker{\nprivate:\n \n std::vector* pyramid1;\n std::vector* pyramid2;\n KLT_TrackingContext tc;\n KLT_FeatureList featurelist;\n float subsampling;\n\npublic:\n \n KLTInvoker(std::vector* _pyramid1,\n std::vector* _pyramid2,\n KLT_TrackingContext _tc,\n KLT_FeatureList _featurelist,\n float _subsampling){\n \n pyramid1=_pyramid1;\n pyramid2=_pyramid2;\n tc=_tc;\n featurelist=_featurelist;\n subsampling=_subsampling;\n };\n \n void operator ()(const tbb::blocked_range& range) const;\n};\n\nvoid KLTInvoker::operator()(const tbb::blocked_range& range) const{\n \n for (int indx = range.begin() ; indx < range.end() ; indx++) {\n \n /* Only track features that are not lost */\n if (featurelist->feature[indx]->val >= 0) {\n \n \n float xloc = featurelist->feature[indx]->x;\n float yloc = featurelist->feature[indx]->y;\n \n /* Transform location to coarsest resolution */\n for (int r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) {\n xloc /= subsampling; yloc /= subsampling;\n }\n \n float xlocout,ylocout;\n \n if(compensate_motion){\n cv::Mat ptMat(3,1,CV_64FC1);\n ptMat.at(0)=subsampling*xloc;\n ptMat.at(1)=subsampling*yloc;\n ptMat.at(2)=1.0;\n ptMat=homography*ptMat;\n xlocout = (ptMat.at(0)/ptMat.at(2))/subsampling;\n ylocout = (ptMat.at(1)/ptMat.at(2))/subsampling;\n }else{\n xlocout =xloc;\n ylocout =yloc;\n }\n \n \n int val;\n /* Beginning with coarsest resolution, do ... */\n for (int r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) {\n //printf(\"%d %d\\n\",indx,r);\n /* Track feature at current resolution */\n xloc *= subsampling; yloc *= subsampling;\n xlocout *= subsampling; ylocout *= subsampling;\n \n val = _trackFeature(xloc, yloc,\n &xlocout, &ylocout,\n (*pyramid1)[3*r],\n (*pyramid1)[3*r+1],(*pyramid1)[3*r+2],\n (*pyramid2)[3*r],\n (*pyramid2)[3*r+1],(*pyramid2)[3*r+2],\n tc->window_width, tc->window_height,\n tc->step_factor,\n tc->max_iterations,\n tc->min_determinant,\n tc->min_displacement,\n tc->max_residue,\n tc->lighting_insensitive);\n \n if (val==KLT_SMALL_DET || val==KLT_OOB)\n break;\n }\n \n int ncols=(*pyramid1)[0].cols,nrows=(*pyramid1)[0].rows;\n \n /* Record feature */\n if (val == KLT_OOB) {\n \n featurelist->feature[indx]->x = -1.0;\n featurelist->feature[indx]->y = -1.0;\n featurelist->feature[indx]->val = KLT_OOB;\n \n } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) {\n \n featurelist->feature[indx]->x = -1.0;\n featurelist->feature[indx]->y = -1.0;\n featurelist->feature[indx]->val = KLT_OOB;\n \n } else if (val == KLT_SMALL_DET) {\n featurelist->feature[indx]->x = -1.0;\n featurelist->feature[indx]->y = -1.0;\n featurelist->feature[indx]->val = KLT_SMALL_DET;\n \n } else if (val == KLT_LARGE_RESIDUE) {\n featurelist->feature[indx]->x = -1.0;\n featurelist->feature[indx]->y = -1.0;\n featurelist->feature[indx]->val = KLT_LARGE_RESIDUE;\n } else if (val == KLT_MAX_ITERATIONS) {\n featurelist->feature[indx]->x = -1.0;\n featurelist->feature[indx]->y = -1.0;\n featurelist->feature[indx]->val = KLT_MAX_ITERATIONS;\n } else {\n featurelist->feature[indx]->x = xlocout;\n featurelist->feature[indx]->y = ylocout;\n featurelist->feature[indx]->val = KLT_TRACKED;\n \n /*if(featurelist->feature[indx]->aff_x==-1.0||featurelist->feature[indx]->aff_y==-1.0){\n \n if(featurelist->feature[indx]->aff_img==NULL){\n featurelist->feature[indx]->aff_img=new cv::Mat(tc->affine_window_height,tc->affine_window_width,CV_32F);\n featurelist->feature[indx]->aff_gradx=new cv::Mat(tc->affine_window_height,tc->affine_window_width,CV_32F);\n featurelist->feature[indx]->aff_grady=new cv::Mat(tc->affine_window_height,tc->affine_window_width,CV_32F);\n }\n \n cv::Mat((*pyramid1)[0],cv::Rect(int(xloc-tc->affine_window_width/2),\n int(yloc-tc->affine_window_height/2),\n tc->affine_window_width,tc->affine_window_height)).copyTo(*featurelist->feature[indx]->aff_img);\n \n cv::Mat((*pyramid1)[1],cv::Rect(int(xloc-tc->affine_window_width/2),\n int(yloc-tc->affine_window_height/2),\n tc->affine_window_width,tc->affine_window_height)).copyTo(*featurelist->feature[indx]->aff_gradx);\n \n cv::Mat((*pyramid1)[2],cv::Rect(int(xloc-tc->affine_window_width/2),\n int(yloc-tc->affine_window_height/2),\n tc->affine_window_width,tc->affine_window_height)).copyTo(*featurelist->feature[indx]->aff_grady);\n \n featurelist->feature[indx]->aff_x=featurelist->feature[indx]->aff_y=0.0;\n \n \n \n }else{\n \n \n \n int val=refineFeatureTranslation(&xlocout,&ylocout,\n *featurelist->feature[indx]->aff_img,\n *featurelist->feature[indx]->aff_gradx,\n *featurelist->feature[indx]->aff_grady,\n (*pyramid2)[0],(*pyramid2)[1],(*pyramid2)[2],\n tc->affine_window_width,tc->affine_window_height,\n tc->step_factor,tc->affine_max_iterations,tc->min_determinant,\n tc->min_displacement,tc->affine_max_residue,tc->affine_max_displacement_differ);\n \n \n featurelist->feature[indx]->val = val;\n \n if(val != KLT_TRACKED){\n featurelist->feature[indx]->x = -1.0;\n featurelist->feature[indx]->y = -1.0;\n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n }else{\n \n }\n }*/\n\n }\n \n }\n }\n};\n\nvoid KLTTrackFeatures(KLT_TrackingContext tc,\n std::vector &pyramid1,\n std::vector &pyramid2,\n KLT_FeatureList featurelist,const Eigen::Matrix3d &invK)\n{\n \n float subsampling = (float) tc->subsampling;\n int ncols=pyramid1[0].cols,nrows=pyramid1[0].rows;\n \n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n \n if (tc->affine_window_width % 2 != 1) {\n tc->affine_window_width = tc->affine_window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->affine_window_width);\n }\n if (tc->affine_window_height % 2 != 1) {\n tc->affine_window_height = tc->affine_window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->affine_window_height);\n }\n \n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"Tracking context's window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"Tracking context's window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n \n tbb::parallel_for(tbb::blocked_range(0,featurelist->nFeatures),\n KLTInvoker(&pyramid1,&pyramid2,tc,featurelist,subsampling));\n \n std::vector pts1,pts2;\n for (int i=0;inFeatures;i++) {\n \n if(featurelist->feature[i]->val!=KLT_TRACKED){\n continue;\n }\n \n Eigen::Vector3d pt((double)featurelist->feature[i]->x,(double)featurelist->feature[i]->y,1.0);\n pt=invK*pt;\n \n featurelist->feature[i]->norm=pt;\n featurelist->feature[i]->norm.normalize();\n \n \n featurelist->feature[i]->vec=featurelist->feature[i]->norm;\n featurelist->feature[i]->vec/=featurelist->feature[i]->vec(2);\n \n }\n featurelist->isOutlierRejected=false;\n \n if (KLT_verbose >= 1) {\n fprintf(stderr, \"\\n\\t%d features successfully tracked.\\n\",\n KLTCountRemainingFeatures(featurelist));\n if (tc->writeInternalImages)\n fprintf(stderr, \"\\tWrote images to 'kltimg_tf*.pgm'.\\n\");\n fflush(stderr);\n }\n \n}\n#include \nvoid compensateLightingAndMotion(const std::vector& pts1,\n const std::vector& pts2,\n const std::vector& intensity1,\n const std::vector& intensity2){\n //compensate motion\n int good_count;\n std::vector status;\n homography=GSLAM::EstimateHomography(pts1,pts2,status,good_count);\n \n //compensate lighting\n double error0=0;\n double error=0;\n double light_error0=0.0;\n double light_error=0.0;;\n \n int inlier_count=0;\n Eigen::MatrixXd A=Eigen::MatrixX2d(good_count,2);\n Eigen::VectorXd b=Eigen::VectorXd(good_count);\n \n for(int i=0;ilight_error0){\n lighting_alpha=1.0;\n lighting_beta=0.0;\n }\n \n static std::ofstream record(\"/Users/chaos/Desktop/debug/lighting.txt\");\n record<\n#include \n\nnamespace GSLAM{\n \n class KeyFrame;\n \n\n class LocalMapPoint{\n \n public:\n \n Eigen::Vector3d globalPosition;\n unsigned char color[3];\n unsigned char vcolor[3];\n \n Eigen::Vector3d vec;\n Eigen::Vector3d norm;\n double invdepth;\n \n std::vector errors;\n std::vector vecs;\n std::vector pVectors;\n std::vector tracked;\n \n bool isDeleted;\n bool isFullTrack;\n bool isEstimated;\n int measurementCount;\n \n float mfMinDistance;\n float minLevelScaleFactor;\n \n float mfMaxDistance;\n float maxLevelScaleFactor;\n \n Eigen::Vector3d getPosition() const{\n return vec/invdepth;\n }\n \n bool isValid(){\n return measurementCount>0;\n }\n \n int PredictScale(const float ¤tDist,const float &logScaleFactor){\n float ratio= mfMaxDistance/currentDist;\n return int(std::ceil(std::log(ratio)/logScaleFactor));\n }\n \n float GetMinDistanceInvariance(){\n return 0.8f*mfMinDistance;\n }\n \n float GetMaxDistanceInvariance(){\n return 1.2f*mfMaxDistance;\n }\n \n void updateNormalAndDepth(){\n const float dist = (float)vec.norm()/invdepth;\n mfMaxDistance = dist*maxLevelScaleFactor;\n mfMinDistance = mfMaxDistance/minLevelScaleFactor;\n }\n \n int globalIndex;\n };\n \n}\n#endif\n" }, { "path": "GSLAM/ORBVocabulary.h", "content": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n#ifndef ORBVOCABULARY_H\n#define ORBVOCABULARY_H\n\n\n#include \"./DBoW2/FORB.h\"\n#include \"./DBoW2/TemplatedVocabulary.h\"\n\nnamespace GSLAM{\n typedef DBoW2::TemplatedVocabulary ORBVocabulary;\n}\n\n#endif\n\n" }, { "path": "GSLAM/ORBextractor.cc", "content": "/**\n* This file is part of ORB-SLAM2.\n* This file is based on the file orb.cpp from the OpenCV library (see BSD license below).\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n/**\n* Software License Agreement (BSD License)\n*\n* Copyright (c) 2009, Willow Garage, Inc.\n* All rights reserved.\n*\n* Redistribution and use in source and binary forms, with or without\n* modification, are permitted provided that the following conditions\n* are met:\n*\n* * Redistributions of source code must retain the above copyright\n* notice, this list of conditions and the following disclaimer.\n* * Redistributions in binary form must reproduce the above\n* copyright notice, this list of conditions and the following\n* disclaimer in the documentation and/or other materials provided\n* with the distribution.\n* * Neither the name of the Willow Garage nor the names of its\n* contributors may be used to endorse or promote products derived\n* from this software without specific prior written permission.\n*\n* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n* \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n* POSSIBILITY OF SUCH DAMAGE.\n*\n*/\n\n\n#include \n//#include \n#include \n#include \n#include \n\n#include \"ORBextractor.h\"\n\n\nusing namespace cv;\nusing namespace std;\n\nnamespace GSLAM\n{\n\nconst int PATCH_SIZE = 31;\nconst int HALF_PATCH_SIZE = 15;\n//const int EDGE_THRESHOLD = 19;\nconst int EDGE_THRESHOLD = 25;\n \n static void\n HarrisResponses(const Mat& img, vector& pts, int blockSize, float harris_k)\n {\n CV_Assert( img.type() == CV_8UC1 && blockSize*blockSize <= 2048 );\n \n size_t ptidx, ptsize = pts.size();\n \n const uchar* ptr00 = img.ptr();\n int step = (int)(img.step/img.elemSize1());\n int r = blockSize/2;\n \n float scale = (1 << 2) * blockSize * 255.0f;\n scale = 1.0f / scale;\n float scale_sq_sq = scale * scale * scale * scale;\n \n AutoBuffer ofsbuf(blockSize*blockSize);\n int* ofs = ofsbuf;\n for( int i = 0; i < blockSize; i++ )\n for( int j = 0; j < blockSize; j++ )\n ofs[i*blockSize + j] = (int)(i*step + j);\n \n for( ptidx = 0; ptidx < ptsize; ptidx++ )\n {\n int x0 = cvRound(pts[ptidx].pt.x - r);\n int y0 = cvRound(pts[ptidx].pt.y - r);\n \n const uchar* ptr0 = ptr00 + y0*step + x0;\n int a = 0, b = 0, c = 0;\n \n for( int k = 0; k < blockSize*blockSize; k++ )\n {\n const uchar* ptr = ptr0 + ofs[k];\n int Ix = (ptr[1] - ptr[-1])*2 + (ptr[-step+1] - ptr[-step-1]) + (ptr[step+1] - ptr[step-1]);\n int Iy = (ptr[step] - ptr[-step])*2 + (ptr[step-1] - ptr[-step-1]) + (ptr[step+1] - ptr[-step+1]);\n a += Ix*Ix;\n b += Iy*Iy;\n c += Ix*Iy;\n }\n pts[ptidx].response = ((float)a * b - (float)c * c -\n harris_k * ((float)a + b) * ((float)a + b))*scale_sq_sq;\n }\n }\n\nstatic float IC_Angle(const Mat& image, Point2f pt, const vector & u_max)\n{\n int m_01 = 0, m_10 = 0;\n\n const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x));\n\n // Treat the center line differently, v=0\n for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)\n m_10 += u * center[u];\n\n // Go line by line in the circuI853lar patch\n int step = (int)image.step1();\n for (int v = 1; v <= HALF_PATCH_SIZE; ++v)\n {\n // Proceed over the two lines\n int v_sum = 0;\n int d = u_max[v];\n for (int u = -d; u <= d; ++u)\n {\n int val_plus = center[u + v*step], val_minus = center[u - v*step];\n v_sum += (val_plus - val_minus);\n m_10 += u * (val_plus + val_minus);\n }\n m_01 += v * v_sum;\n }\n\n return fastAtan2((float)m_01, (float)m_10);\n}\n\n\nconst float factorPI = (float)(CV_PI/180.f);\nstatic void computeOrbDescriptor(const KeyPoint& kpt,\n const Mat& img, const Point* pattern,\n uchar* desc)\n{\n float angle = (float)kpt.angle*factorPI;\n float a = (float)cos(angle), b = (float)sin(angle);\n\n const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x));\n const int step = (int)img.step;\n\n #define GET_VALUE(idx) \\\n center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \\\n cvRound(pattern[idx].x*a - pattern[idx].y*b)]\n\n\n for (int i = 0; i < 32; ++i, pattern += 16)\n {\n int t0, t1, val;\n t0 = GET_VALUE(0); t1 = GET_VALUE(1);\n val = t0 < t1;\n t0 = GET_VALUE(2); t1 = GET_VALUE(3);\n val |= (t0 < t1) << 1;\n t0 = GET_VALUE(4); t1 = GET_VALUE(5);\n val |= (t0 < t1) << 2;\n t0 = GET_VALUE(6); t1 = GET_VALUE(7);\n val |= (t0 < t1) << 3;\n t0 = GET_VALUE(8); t1 = GET_VALUE(9);\n val |= (t0 < t1) << 4;\n t0 = GET_VALUE(10); t1 = GET_VALUE(11);\n val |= (t0 < t1) << 5;\n t0 = GET_VALUE(12); t1 = GET_VALUE(13);\n val |= (t0 < t1) << 6;\n t0 = GET_VALUE(14); t1 = GET_VALUE(15);\n val |= (t0 < t1) << 7;\n\n desc[i] = (uchar)val;\n }\n\n #undef GET_VALUE\n}\n\n\nstatic int bit_pattern_31_[256*4] =\n{\n 8,-3, 9,5/*mean (0), correlation (0)*/,\n 4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,\n -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/,\n 7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/,\n 2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/,\n 1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/,\n -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/,\n -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/,\n -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/,\n 10,4, 11,9/*mean (0.122065), correlation (0.093285)*/,\n -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/,\n -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/,\n 7,7, 12,6/*mean (0.160583), correlation (0.130064)*/,\n -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/,\n -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/,\n -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/,\n 12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/,\n -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/,\n -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/,\n 11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/,\n 4,7, 5,1/*mean (0.205106), correlation (0.186848)*/,\n 5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/,\n 3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/,\n -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/,\n -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/,\n -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/,\n -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/,\n -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/,\n -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/,\n 5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/,\n 5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/,\n 1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/,\n 9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/,\n 4,7, 4,12/*mean (0.131005), correlation (0.257622)*/,\n 2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/,\n -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/,\n -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/,\n 4,11, 9,12/*mean (0.226226), correlation (0.258255)*/,\n 0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/,\n -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/,\n -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/,\n -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/,\n 8,12, 10,7/*mean (0.225337), correlation (0.282851)*/,\n 0,9, 1,3/*mean (0.226687), correlation (0.278734)*/,\n 7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/,\n -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/,\n 10,7, 12,1/*mean (0.125517), correlation (0.31089)*/,\n -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/,\n 10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/,\n -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/,\n -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/,\n 3,3, 7,8/*mean (0.177755), correlation (0.309394)*/,\n 5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/,\n -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/,\n 3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/,\n 2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/,\n -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/,\n -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/,\n -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/,\n -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/,\n 6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/,\n -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/,\n -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/,\n -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/,\n 3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/,\n -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/,\n -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/,\n 2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/,\n -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/,\n -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/,\n 5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/,\n -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/,\n -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/,\n -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/,\n 10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/,\n 7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/,\n -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/,\n -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/,\n 7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/,\n -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/,\n -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/,\n -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/,\n 7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/,\n -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/,\n 1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/,\n 2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/,\n -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/,\n -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/,\n 7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/,\n 1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/,\n 9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/,\n -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/,\n -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/,\n 7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/,\n 12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/,\n 6,3, 7,11/*mean (0.1074), correlation (0.413224)*/,\n 5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/,\n 2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/,\n 3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/,\n 2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/,\n 9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/,\n -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/,\n -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/,\n 1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/,\n 6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/,\n 2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/,\n 6,3, 11,0/*mean (0.204588), correlation (0.411762)*/,\n 3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/,\n 7,8, 9,3/*mean (0.213237), correlation (0.409306)*/,\n -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/,\n -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/,\n -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/,\n -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/,\n 8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/,\n 4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/,\n -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/,\n 4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/,\n -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/,\n -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/,\n 7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/,\n -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/,\n -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/,\n 8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/,\n -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/,\n 1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/,\n 7,-4, 9,1/*mean (0.132692), correlation (0.454)*/,\n -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/,\n 11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/,\n -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/,\n 3,7, 7,12/*mean (0.147627), correlation (0.456643)*/,\n 5,5, 10,8/*mean (0.152901), correlation (0.455036)*/,\n 0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/,\n -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/,\n 0,7, 2,12/*mean (0.18312), correlation (0.433855)*/,\n -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/,\n 5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/,\n 3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/,\n -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/,\n -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/,\n -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/,\n 6,5, 8,0/*mean (0.1972), correlation (0.450481)*/,\n -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/,\n -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/,\n 1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/,\n 4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/,\n -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/,\n 2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/,\n -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/,\n 4,1, 9,3/*mean (0.23962), correlation (0.444824)*/,\n -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/,\n -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/,\n 7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/,\n 4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/,\n -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/,\n 7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/,\n 7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/,\n -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/,\n -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/,\n -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/,\n 2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/,\n 10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/,\n -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/,\n 8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/,\n 2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/,\n -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/,\n -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/,\n -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/,\n 5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/,\n -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/,\n -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/,\n -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/,\n -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/,\n -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/,\n 2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/,\n -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/,\n -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/,\n -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/,\n -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/,\n 6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/,\n -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/,\n 11,11, 12,6/*mean (0.16652), correlation (0.497632)*/,\n 7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/,\n -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/,\n -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/,\n -7,1, -6,7/*mean (0.175), correlation (0.500024)*/,\n -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/,\n -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/,\n -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/,\n -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/,\n -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/,\n 1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/,\n 1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/,\n 9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/,\n 5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/,\n -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/,\n -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/,\n -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/,\n -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/,\n 8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/,\n 2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/,\n 7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/,\n -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/,\n -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/,\n 4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/,\n 3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/,\n -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/,\n 5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/,\n 4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/,\n -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/,\n 0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/,\n -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/,\n 3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/,\n -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/,\n 8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/,\n -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/,\n 2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/,\n 10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/,\n 6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/,\n -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/,\n -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/,\n -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/,\n -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/,\n -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/,\n 4,2, 12,12/*mean (0.01778), correlation (0.546921)*/,\n 2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/,\n 6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/,\n 3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/,\n 11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/,\n -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/,\n 4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/,\n 2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/,\n -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/,\n -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/,\n -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/,\n 6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/,\n 0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/,\n -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/,\n -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/,\n -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/,\n 5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/,\n 2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/,\n -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/,\n 9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/,\n 11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/,\n 3,0, 3,5/*mean (0.101147), correlation (0.525576)*/,\n -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/,\n 3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/,\n -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/,\n 5,8, 12,11/*mean (0.114181), correlation (0.555793)*/,\n 8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/,\n 7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/,\n -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/,\n 7,3, 12,4/*mean (0.122582), correlation (0.555825)*/,\n 9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/,\n 7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/,\n -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/\n};\n\nORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,\n int _iniThFAST, int _minThFAST):\n nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),\n iniThFAST(_iniThFAST), minThFAST(_minThFAST)\n{\n mvScaleFactor.resize(nlevels);\n mvLevelSigma2.resize(nlevels);\n mvScaleFactor[0]=1.0f;\n mvLevelSigma2[0]=1.0f;\n for(int i=1; i= vmin; --v)\n {\n while (umax[v0] == umax[v0 + 1])\n ++v0;\n umax[v] = v0;\n ++v0;\n }\n}\n\nstatic void computeOrientation(const Mat& image, vector& keypoints, const vector& umax)\n{\n for (vector::iterator keypoint = keypoints.begin(),\n keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){\n keypoint->angle = IC_Angle(image, keypoint->pt, umax);\n }\n}\n\nvoid ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)\n{\n const int halfX = ceil(static_cast(UR.x-UL.x)/2);\n const int halfY = ceil(static_cast(BR.y-UL.y)/2);\n\n //Define boundaries of childs\n n1.UL = UL;\n n1.UR = cv::Point2i(UL.x+halfX,UL.y);\n n1.BL = cv::Point2i(UL.x,UL.y+halfY);\n n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);\n n1.vKeys.reserve(vKeys.size());\n\n n2.UL = n1.UR;\n n2.UR = UR;\n n2.BL = n1.BR;\n n2.BR = cv::Point2i(UR.x,UL.y+halfY);\n n2.vKeys.reserve(vKeys.size());\n\n n3.UL = n1.BL;\n n3.UR = n1.BR;\n n3.BL = BL;\n n3.BR = cv::Point2i(n1.BR.x,BL.y);\n n3.vKeys.reserve(vKeys.size());\n\n n4.UL = n3.UR;\n n4.UR = n2.BR;\n n4.BL = n3.BR;\n n4.BR = BR;\n n4.vKeys.reserve(vKeys.size());\n\n //Associate points to childs\n for(size_t i=0;i ORBextractor::DistributeOctTree(const vector& vToDistributeKeys, const int &minX,\n const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)\n{\n // Compute how many initial nodes \n const int nIni = round(static_cast(maxX-minX)/(maxY-minY));\n\n const float hX = static_cast(maxX-minX)/nIni;\n\n list lNodes;\n\n vector vpIniNodes;\n vpIniNodes.resize(nIni);\n\n for(int i=0; i(i),0);\n ni.UR = cv::Point2i(hX*static_cast(i+1),0);\n ni.BL = cv::Point2i(ni.UL.x,maxY-minY);\n ni.BR = cv::Point2i(ni.UR.x,maxY-minY);\n ni.vKeys.reserve(vToDistributeKeys.size());\n\n lNodes.push_back(ni);\n vpIniNodes[i] = &lNodes.back();\n }\n\n //Associate points to childs\n for(size_t i=0;ivKeys.push_back(kp);\n }\n\n list::iterator lit = lNodes.begin();\n\n while(lit!=lNodes.end())\n {\n if(lit->vKeys.size()==1)\n {\n lit->bNoMore=true;\n lit++;\n }\n else if(lit->vKeys.empty())\n lit = lNodes.erase(lit);\n else\n lit++;\n }\n\n bool bFinish = false;\n\n int iteration = 0;\n\n vector > vSizeAndPointerToNode;\n vSizeAndPointerToNode.reserve(lNodes.size()*4);\n\n while(!bFinish)\n {\n iteration++;\n\n int prevSize = lNodes.size();\n\n lit = lNodes.begin();\n\n int nToExpand = 0;\n\n vSizeAndPointerToNode.clear();\n\n while(lit!=lNodes.end())\n {\n if(lit->bNoMore)\n {\n // If node only contains one point do not subdivide and continue\n lit++;\n continue;\n }\n else\n {\n // If more than one point, subdivide\n ExtractorNode n1,n2,n3,n4;\n lit->DivideNode(n1,n2,n3,n4);\n\n // Add childs if they contain points\n if(n1.vKeys.size()>0)\n {\n lNodes.push_front(n1); \n if(n1.vKeys.size()>1)\n {\n nToExpand++;\n vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n if(n2.vKeys.size()>0)\n {\n lNodes.push_front(n2);\n if(n2.vKeys.size()>1)\n {\n nToExpand++;\n vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n if(n3.vKeys.size()>0)\n {\n lNodes.push_front(n3);\n if(n3.vKeys.size()>1)\n {\n nToExpand++;\n vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n if(n4.vKeys.size()>0)\n {\n lNodes.push_front(n4);\n if(n4.vKeys.size()>1)\n {\n nToExpand++;\n vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n\n lit=lNodes.erase(lit);\n continue;\n }\n } \n\n // Finish if there are more nodes than required features\n // or all nodes contain just one point\n if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)\n {\n bFinish = true;\n }\n else if(((int)lNodes.size()+nToExpand*3)>N)\n {\n\n while(!bFinish)\n {\n\n prevSize = lNodes.size();\n\n vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;\n vSizeAndPointerToNode.clear();\n\n sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());\n for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)\n {\n ExtractorNode n1,n2,n3,n4;\n vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);\n\n // Add childs if they contain points\n if(n1.vKeys.size()>0)\n {\n lNodes.push_front(n1);\n if(n1.vKeys.size()>1)\n {\n vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n if(n2.vKeys.size()>0)\n {\n lNodes.push_front(n2);\n if(n2.vKeys.size()>1)\n {\n vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n if(n3.vKeys.size()>0)\n {\n lNodes.push_front(n3);\n if(n3.vKeys.size()>1)\n {\n vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n if(n4.vKeys.size()>0)\n {\n lNodes.push_front(n4);\n if(n4.vKeys.size()>1)\n {\n vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));\n lNodes.front().lit = lNodes.begin();\n }\n }\n\n lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);\n\n if((int)lNodes.size()>=N)\n break;\n }\n\n if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)\n bFinish = true;\n\n }\n }\n }\n\n // Retain the best point in each node\n vector vResultKeys;\n vResultKeys.reserve(nfeatures);\n for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)\n {\n vector &vNodeKeys = lit->vKeys;\n cv::KeyPoint* pKP = &vNodeKeys[0];\n float maxResponse = pKP->response;\n\n for(size_t k=1;kmaxResponse)\n {\n pKP = &vNodeKeys[k];\n maxResponse = vNodeKeys[k].response;\n }\n }\n\n vResultKeys.push_back(*pKP);\n }\n\n return vResultKeys;\n}\n\nvoid ORBextractor::ComputeKeyPointsOctTree(vector >& allKeypoints)\n{\n allKeypoints.resize(nlevels);\n\n const float W = 30;\n \n //Ptr detector=GFTTDetector::create();\n \n for (int level = 0; level < nlevels; ++level)\n {\n const int minBorderX = EDGE_THRESHOLD-3;\n const int minBorderY = minBorderX;\n const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;\n const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;\n\n vector vToDistributeKeys;\n vToDistributeKeys.reserve(nfeatures*10);\n\n const float width = (maxBorderX-minBorderX);\n const float height = (maxBorderY-minBorderY);\n\n const int nCols = width/W;\n const int nRows = height/W;\n const int wCell = ceil(width/nCols);\n const int hCell = ceil(height/nRows);\n\n for(int i=0; i=maxBorderY-3)\n continue;\n if(maxY>maxBorderY)\n maxY = maxBorderY;\n\n for(int j=0; j=maxBorderX-6)\n continue;\n if(maxX>maxBorderX)\n maxX = maxBorderX;\n\n vector vKeysCell;\n\n FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),\n vKeysCell,iniThFAST,true);\n \n if(vKeysCell.empty()){\n FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),\n vKeysCell,minThFAST,true);\n }\n \n /*detector->detect(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),\n vKeysCell);*/\n\n if(!vKeysCell.empty())\n {\n for(vector::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)\n {\n (*vit).pt.x+=j*wCell;\n (*vit).pt.y+=i*hCell;\n vToDistributeKeys.push_back(*vit);\n }\n }\n\n }\n }\n\n vector & keypoints = allKeypoints[level];\n keypoints.reserve(nfeatures);\n\n keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,\n minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);\n\n const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];\n\n // Add border to coordinates and scale information\n const int nkps = keypoints.size();\n for(int i=0; i > &allKeypoints)\n{\n allKeypoints.resize(nlevels);\n\n float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows;\n\n for (int level = 0; level < nlevels; ++level)\n {\n const int nDesiredFeatures = mnFeaturesPerLevel[level];\n\n const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio));\n const int levelRows = imageRatio*levelCols;\n\n const int minBorderX = EDGE_THRESHOLD;\n const int minBorderY = minBorderX;\n const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD;\n const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD;\n\n const int W = maxBorderX - minBorderX;\n const int H = maxBorderY - minBorderY;\n const int cellW = ceil((float)W/levelCols);\n const int cellH = ceil((float)H/levelRows);\n\n const int nCells = levelRows*levelCols;\n const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells);\n\n vector > > cellKeyPoints(levelRows, vector >(levelCols));\n\n vector > nToRetain(levelRows,vector(levelCols,0));\n vector > nTotal(levelRows,vector(levelCols,0));\n vector > bNoMore(levelRows,vector(levelCols,false));\n vector iniXCol(levelCols);\n vector iniYRow(levelRows);\n int nNoMore = 0;\n int nToDistribute = 0;\n\n\n float hY = cellH + 6;\n\n for(int i=0; infeaturesCell)\n {\n nToRetain[i][j] = nfeaturesCell;\n bNoMore[i][j] = false;\n }\n else\n {\n nToRetain[i][j] = nKeys;\n nToDistribute += nfeaturesCell-nKeys;\n bNoMore[i][j] = true;\n nNoMore++;\n }\n\n }\n }\n\n\n // Retain by score\n\n while(nToDistribute>0 && nNoMorenNewFeaturesCell)\n {\n nToRetain[i][j] = nNewFeaturesCell;\n bNoMore[i][j] = false;\n }\n else\n {\n nToRetain[i][j] = nTotal[i][j];\n nToDistribute += nNewFeaturesCell-nTotal[i][j];\n bNoMore[i][j] = true;\n nNoMore++;\n }\n }\n }\n }\n }\n\n vector & keypoints = allKeypoints[level];\n keypoints.reserve(nDesiredFeatures*2);\n\n const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];\n\n // Retain by score and transform coordinates\n for(int i=0; i &keysCell = cellKeyPoints[i][j];\n KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]);\n if((int)keysCell.size()>nToRetain[i][j])\n keysCell.resize(nToRetain[i][j]);\n\n\n for(size_t k=0, kend=keysCell.size(); knDesiredFeatures)\n {\n KeyPointsFilter::retainBest(keypoints,nDesiredFeatures);\n keypoints.resize(nDesiredFeatures);\n }\n }\n\n // and compute orientations\n for (int level = 0; level < nlevels; ++level)\n computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);\n}\n\n void ORBextractor::ComputeKeyPointsEigVal(cv::Mat dx,cv::Mat dy,std::vector >& allKeypoints){\n \n }\n\nstatic void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors,\n const vector& pattern)\n{\n descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);\n\n for (size_t i = 0; i < keypoints.size(); i++){\n //printf(\"%d %d %f %f\\n\",image.cols,image.rows,keypoints[i].pt.x,keypoints[i].pt.y);\n computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));\n }\n}\n\nvoid ORBextractor::operator()(InputArray _image, InputArray _mask,\n vector& _keypoints,OutputArray _descriptors){\n \n if(_image.empty())\n return;\n\n Mat image = _image.getMat();\n assert(image.type() == CV_8UC1 );\n\n // Pre-compute the scale pyramid\n ComputePyramid(image);\n\n vector < vector > allKeypoints;\n ComputeKeyPointsOctTree(allKeypoints);\n \n Mat descriptors;\n\n int nkeypoints = 0;\n for (int level = 0; level < nlevels; ++level)\n nkeypoints += (int)allKeypoints[level].size();\n if( nkeypoints == 0 )\n _descriptors.release();\n else\n {\n _descriptors.create(nkeypoints, 32, CV_8U);\n descriptors = _descriptors.getMat();\n }\n\n _keypoints.clear();\n _keypoints.reserve(nkeypoints);\n\n int offset = 0;\n for (int level = 0; level < nlevels; ++level)\n {\n vector& keypoints = allKeypoints[level];\n int nkeypointsLevel = (int)keypoints.size();\n\n if(nkeypointsLevel==0)\n continue;\n\n // preprocess the resized image\n Mat workingMat = mvImagePyramid[level].clone();\n GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);\n\n // Compute the descriptors\n Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);\n computeDescriptors(workingMat, keypoints, desc, pattern);\n\n offset += nkeypointsLevel;\n\n // Scale keypoint coordinates\n if (level != 0){\n \n float scale = mvScaleFactor[level];\n \n for (vector::iterator keypoint = keypoints.begin(),\n keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){\n keypoint->pt *= scale;\n }\n }\n // And add the keypoints to the output\n _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());\n }\n}\n\nvoid ORBextractor::ComputePyramid(cv::Mat image)\n{\n for (int level = 0; level < nlevels; ++level)\n {\n float scale = mvInvScaleFactor[level];\n Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));\n Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);\n Mat temp(wholeSize, image.type()), masktemp;\n mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));\n\n // Compute the resized image\n if( level != 0 ){\n resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);\n\n copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,\n BORDER_REFLECT_101+BORDER_ISOLATED); \n }\n else{\n copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,\n BORDER_REFLECT_101); \n }\n }\n\n}\n\n} //namespace ORB_SLAM\n" }, { "path": "GSLAM/ORBextractor.h", "content": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n#ifndef ORBEXTRACTOR_H\n#define ORBEXTRACTOR_H\n\n#include \n#include \n//#include \n#include \"opencv2/core/core_c.h\"\n#include \"opencv2/imgproc/imgproc_c.h\"\n\nnamespace GSLAM\n{\n\nclass ExtractorNode\n{\npublic:\n ExtractorNode():bNoMore(false){}\n\n void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4);\n\n std::vector vKeys;\n cv::Point2i UL, UR, BL, BR;\n std::list::iterator lit;\n bool bNoMore;\n};\n\nclass ORBextractor\n{\npublic:\n \n enum {HARRIS_SCORE=0, FAST_SCORE=1 };\n\n ORBextractor(int nfeatures, float scaleFactor, int nlevels,\n int iniThFAST, int minThFAST);\n\n ~ORBextractor(){}\n\n // Compute the ORB features and descriptors on an image.\n // ORB are dispersed on the image using an octree.\n // Mask is ignored in the current implementation.\n void operator()( cv::InputArray image, cv::InputArray mask,\n std::vector& keypoints,\n cv::OutputArray descriptors);\n\n int inline GetLevels(){\n return nlevels;\n }\n\n float inline GetScaleFactor(){\n return scaleFactor;}\n\n std::vector inline GetScaleFactors(){\n return mvScaleFactor;\n }\n\n std::vector inline GetInverseScaleFactors(){\n return mvInvScaleFactor;\n }\n\n std::vector inline GetScaleSigmaSquares(){\n return mvLevelSigma2;\n }\n\n std::vector inline GetInverseScaleSigmaSquares(){\n return mvInvLevelSigma2;\n }\n\n std::vector mvImagePyramid;\n\nprotected:\n\n void ComputePyramid(cv::Mat image);\n \n void ComputeKeyPointsOctTree(std::vector >& allKeypoints); \n std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX,\n const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);\n\n void ComputeKeyPointsOld(std::vector >& allKeypoints);\n void ComputeKeyPointsUniform(std::vector >& allKeypoints);\n void ComputeKeyPointsEigVal(cv::Mat dx,cv::Mat dy,std::vector >& allKeypoints);\n \n std::vector pattern;\n int nfeatures;\n double scaleFactor;\n int nlevels;\n int iniThFAST;\n int minThFAST;\n\n std::vector mnFeaturesPerLevel;\n\n std::vector umax;\n\n std::vector mvScaleFactor;\n std::vector mvInvScaleFactor; \n std::vector mvLevelSigma2;\n std::vector mvInvLevelSigma2;\n};\n\n} //namespace GSLAM\n#endif\n\n" }, { "path": "GSLAM/ORBmatcher.cc", "content": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n\n#include \"ORBmatcher.h\"\n#include \n#include \"opencv2/core/core.hpp\"\n#include \"opencv2/features2d/features2d.hpp\"\n#include \"./DBoW2/FeatureVector.h\"\n#include \n#include \n#include \"Geometry.h\"\n\nusing namespace std;\n\nnamespace GSLAM\n{\n\n const int ORBmatcher::TH_HIGH = 100;\n const int ORBmatcher::TH_LOW = 64;\n const int ORBmatcher::HISTO_LENGTH = 30;\n \n enum{\n NOT_MATCHED=-1,\n NO_MATCH_CANDIDATE=-2,\n LARGE_MATCH_DISTANCE=-3,\n OUT_OF_DISTANCE_RANGE=-4,\n ROTATION_INCONSISTENT=-5\n };\n \n\n ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbCheckOrientation(checkOri){\n\n\n }\n \n bool errorCompare(const pair& e1, const pair& e2) {\n return e1.second < e2.second;\n }\n \n \n int ORBmatcher::SearchByRotation(Frame* frame1,Frame* frame2,\n const cv::Mat& rotation,\n std::vector& pts1,\n std::vector& pts2,\n int ORBdist){\n \n int nMatches=0;\n std::vector matches12(frame1->mvKeys.size(),-1);\n std::vector matches21(frame2->mvKeys.size(),-1);\n pts1.clear();\n pts2.clear();\n \n for (int p=0;pmvKeys.size();p++) {\n \n if (matches12[p]>=0) {\n nMatches++;\n continue;\n }\n \n if (true) {\n \n cv::Mat ptMat(3,1,CV_64FC1);\n ptMat.at(0)=frame1->mvKeys[p].pt.x;\n ptMat.at(1)=frame1->mvKeys[p].pt.y;\n ptMat.at(2)=1.0;\n ptMat=rotation*ptMat;\n \n float u=(float)(ptMat.at(0)/ptMat.at(2)),\n v=(float)(ptMat.at(1)/ptMat.at(2));\n \n if(umnMinX || u>frame2->mnMaxX)\n continue;\n \n if(vmnMinY || v>frame2->mnMaxY)\n continue;\n \n \n std::vector squareDistances;\n \n const vector vIndices2 = frame2->GetFeaturesInArea(u,v,20.0,0,2,squareDistances);\n \n if(vIndices2.empty()){\n matches12[p]=NO_MATCH_CANDIDATE;\n continue;\n }\n \n const cv::Mat dMP = frame1->mDescriptors.row(p);\n \n int bestDist = 256;\n int bestIdx2 = -1;\n \n for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){\n \n const size_t i2 = *vit;\n \n if(matches21[i2]>=0){\n continue;\n }\n \n const cv::Mat &d = frame2->mDescriptors.row(i2);\n const int dist = DescriptorDistance(dMP,d);\n \n if(distmvKeys[p].pt);\n pts2.push_back(frame2->mvKeys[bestIdx2].pt);\n \n nMatches++;\n }else{\n matches12[p]=LARGE_MATCH_DISTANCE;\n }\n }\n }\n return nMatches;\n }\n \n int ORBmatcher::SearchByTracking(KeyFrame *keyFrame1,KeyFrame *keyFrame2,\n vector &matches12,vector &matches21,\n const int ORBdist){\n \n std::vector > errorIndex;\n for (int p=0;pmvLocalMapPoints.size();p++) {\n if (keyFrame1->mvLocalMapPoints[p].isEstimated\n &&keyFrame1->mvLocalMapPoints[p].isValid()\n &&keyFrame1->mvLocalMapPoints[p].vecs.back()!=NULL) {\n errorIndex.push_back(make_pair(p,keyFrame1->mvLocalMapPoints[p].errors.back()*K(0,0)));\n }\n }\n std::sort(errorIndex.begin(),errorIndex.end(),errorCompare);\n \n int nMatches=0;\n for (int i=0;imvLocalMapPoints[p].vecs.back());\n project/=project(2);\n \n \n const int nPredictedLevel=keyFrame1->framePtr->mvKeysUn[p].octave;\n float radius=3*std::max(error,2.f);\n \n vector squareDistances;\n const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(project(0),project(1),\n radius,nPredictedLevel-2,\n nPredictedLevel+2,\n squareDistances);\n if (vIndices2.empty()) {\n matches12[p]=NO_MATCH_CANDIDATE;\n continue;\n }\n \n const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p);\n int bestDist = 256;\n int bestIdx2 = -1;\n \n for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end();vit++){\n \n const size_t i2 = *vit;\n \n if(matches21[i2]>=0){\n continue;\n }\n \n const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2);\n const int dist = DescriptorDistance(dMP,d);\n \n if(dist &matches12,vector &matches21,const float th,const int ORBdist){\n \n int nMatches=0;\n // Rotation Histogram (to check rotation consistency)\n vector rotHist[HISTO_LENGTH];\n for(int i=0;imvLocalMapPoints.size();p++) {\n \n if (matches12[p]>=0) {\n nMatches++;\n continue;\n }\n\n if (keyFrame1->mvLocalMapPoints[p].isEstimated) {\n \n Eigen::Vector3d position1=keyFrame1->mvLocalMapPoints[p].getPosition();\n Eigen::Vector3d position2=transform.rotation*(keyFrame1->mvLocalMapPoints[p].getPosition()-transform.translation);\n Eigen::Vector3d predict;\n \n predict=K*position2;\n predict/=predict(2);\n \n float u=(float)predict(0),v=(float)predict(1);\n if(uframePtr->mnMinX || u>keyFrame2->framePtr->mnMaxX)\n continue;\n if(vframePtr->mnMinY || v>keyFrame2->framePtr->mnMaxY)\n continue;\n \n \n Eigen::Vector3d PO= position1-transform.translation;\n double dist3D=PO.norm();\n \n const float maxDistance=keyFrame1->mvLocalMapPoints[p].GetMaxDistanceInvariance();\n const float minDistance=keyFrame1->mvLocalMapPoints[p].GetMinDistanceInvariance();\n \n if(dist3DmaxDistance){\n matches12[p]=OUT_OF_DISTANCE_RANGE;\n continue;\n }\n \n int nPredictedLevel =keyFrame1->mvLocalMapPoints[p].PredictScale(dist3D,keyFrame2->framePtr->mfLogScaleFactor);\n nPredictedLevel=nPredictedLevel<0?0:nPredictedLevel;\n nPredictedLevel=nPredictedLevel>=keyFrame1->framePtr->mnScaleLevels?\n keyFrame1->framePtr->mnScaleLevels-1:nPredictedLevel;\n \n \n const float radius = th*keyFrame2->framePtr->mvScaleFactors[nPredictedLevel];\n \n std::vector squareDistances;\n const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(u,v,radius,\n nPredictedLevel-2,nPredictedLevel+2,\n squareDistances);\n if(vIndices2.empty()){\n matches12[p]=NO_MATCH_CANDIDATE;\n continue;\n }\n \n const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p);\n \n int bestDist = 256;\n int bestIdx2 = -1;\n \n for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){\n \n const size_t i2 = *vit;\n \n if(matches21[i2]>=0){\n continue;\n }\n \n const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2);\n const int dist = DescriptorDistance(dMP,d);\n \n if(distframePtr->mvKeysUn[p].angle-keyFrame1->framePtr->mvKeysUn[bestIdx2].angle;\n if(rot<0.0)\n rot+=360.0f;\n int bin = round(rot*factor);\n if(bin==HISTO_LENGTH)\n bin=0;\n assert(bin>=0 && bin &matches12,vector &matches21,\n const float th,const int ORBdist){\n \n int nMatches=0;\n // Rotation Histogram (to check rotation consistency)\n vector rotHist[HISTO_LENGTH];\n for(int i=0;imvLocalMapPoints.size();p++) {\n \n if (matches12[p]>=0) {\n assert(matches21[matches12[p]]==p);\n nMatches++;\n continue;\n }\n \n if (keyFrame1->mvLocalMapPoints[p].isEstimated) {\n \n Eigen::Vector3d position1=keyFrame1->mvLocalMapPoints[p].getPosition();\n Eigen::Vector3d position2=transform.rotation*(keyFrame1->mvLocalMapPoints[p].getPosition()-transform.translation);\n Eigen::Vector3d predict;\n \n predict=K*position2;\n predict/=predict(2);\n \n float u=(float)predict(0),v=(float)predict(1);\n if(uframePtr->mnMinX || u>keyFrame2->framePtr->mnMaxX)\n continue;\n if(vframePtr->mnMinY || v>keyFrame2->framePtr->mnMaxY)\n continue;\n \n \n Eigen::Vector3d PO= position1-transform.translation;\n double dist3D=PO.norm();\n \n const float maxDistance=keyFrame1->mvLocalMapPoints[p].GetMaxDistanceInvariance();\n const float minDistance=keyFrame1->mvLocalMapPoints[p].GetMinDistanceInvariance();\n \n if(dist3DmaxDistance){\n matches12[p]=OUT_OF_DISTANCE_RANGE;\n continue;\n }\n \n int nPredictedLevel =keyFrame1->mvLocalMapPoints[p].PredictScale(dist3D,keyFrame2->framePtr->mfLogScaleFactor);\n nPredictedLevel=nPredictedLevel<0?0:nPredictedLevel;\n nPredictedLevel=nPredictedLevel>=keyFrame1->framePtr->mnScaleLevels?\n keyFrame1->framePtr->mnScaleLevels-1:nPredictedLevel;\n \n \n const float radius = th*keyFrame2->framePtr->mvScaleFactors[nPredictedLevel];\n \n std::vector squareDistances;\n const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(u,v,radius,\n nPredictedLevel-2,nPredictedLevel+2,\n squareDistances);\n if(vIndices2.empty()){\n matches12[p]=NO_MATCH_CANDIDATE;\n continue;\n }\n \n const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p);\n \n int bestDist = 256;\n int bestIdx2 = -1;\n \n for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){\n \n const size_t i2 = *vit;\n \n if(matches21[i2]>=0){\n continue;\n }\n \n const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2);\n const int dist = DescriptorDistance(dMP,d);\n \n if(distframePtr->mvKeysUn[p].angle-keyFrame1->framePtr->mvKeysUn[bestIdx2].angle;\n if(rot<0.0)\n rot+=360.0f;\n int bin = round(rot*factor);\n if(bin==HISTO_LENGTH)\n bin=0;\n assert(bin>=0 && bin &matches12,vector &matches21,\n const float th,const int ORBdist){\n int nMatches=0;\n vector rotHist[HISTO_LENGTH];\n for(int i=0;imvLocalMapPoints.size();p++) {\n \n if (matches12[p]>=0) {\n assert(matches21[matches12[p]]==p);\n nMatches++;\n continue;\n }\n \n if (keyFrame1->mvLocalMapPoints[p].isEstimated) {\n \n Eigen::Vector3d position1=keyFrame1->mvLocalMapPoints[p].getPosition();\n Eigen::Vector3d position2=transform.rotation*(keyFrame1->mvLocalMapPoints[p].getPosition()-transform.translation);\n Eigen::Vector3d predict;\n \n predict=K*position2;\n predict/=predict(2);\n \n float u=(float)predict(0),v=(float)predict(1);\n if(uframePtr->mnMinX || u>keyFrame2->framePtr->mnMaxX)\n continue;\n if(vframePtr->mnMinY || v>keyFrame2->framePtr->mnMaxY)\n continue;\n \n \n Eigen::Vector3d PO= position1-transform.translation;\n double dist3D=PO.norm();\n \n const float maxDistance=keyFrame1->mvLocalMapPoints[p].GetMaxDistanceInvariance();\n const float minDistance=keyFrame1->mvLocalMapPoints[p].GetMinDistanceInvariance();\n \n if(dist3DmaxDistance){\n matches12[p]=OUT_OF_DISTANCE_RANGE;\n continue;\n }\n \n int nPredictedLevel =keyFrame1->mvLocalMapPoints[p].PredictScale(dist3D,keyFrame2->framePtr->mfLogScaleFactor);\n nPredictedLevel=nPredictedLevel<0?0:nPredictedLevel;\n nPredictedLevel=nPredictedLevel>=keyFrame1->framePtr->mnScaleLevels?\n keyFrame1->framePtr->mnScaleLevels-1:nPredictedLevel;\n \n \n const float radius = th*keyFrame2->framePtr->mvScaleFactors[nPredictedLevel];\n \n std::vector squareDistances;\n const vector vIndices2 = keyFrame2->framePtr->GetFeaturesInArea(u,v,radius,\n nPredictedLevel-2,\n nPredictedLevel+2,\n squareDistances);\n if(vIndices2.empty()){\n matches12[p]=NO_MATCH_CANDIDATE;\n continue;\n }\n \n const cv::Mat dMP = keyFrame1->framePtr->mDescriptors.row(p);\n \n int bestDist = 256;\n int bestIdx2 = -1;\n \n for(vector::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++){\n \n const size_t i2 = *vit;\n if(matches21[i2]>=0||!keyFrame2->mvLocalMapPoints[i2].isEstimated){\n continue;\n }\n \n const cv::Mat &d = keyFrame2->framePtr->mDescriptors.row(i2);\n const int dist = DescriptorDistance(dMP,d);\n \n if(distframePtr->mvKeysUn[p].angle-keyFrame1->framePtr->mvKeysUn[bestIdx2].angle;\n if(rot<0.0)\n rot+=360.0f;\n int bin = round(rot*factor);\n if(bin==HISTO_LENGTH)\n bin=0;\n assert(bin>=0 && bin &matches12,vector &matches21)\n {\n const vector &vKeysUn1 = pKF1->framePtr->mvKeysUn;\n const DBoW2::FeatureVector &vFeatVec1 = pKF1->framePtr->mFeatVec;\n const cv::Mat &Descriptors1 = pKF1->framePtr->mDescriptors;\n \n const vector &vKeysUn2 = pKF2->framePtr->mvKeysUn;\n const DBoW2::FeatureVector &vFeatVec2 = pKF2->framePtr->mFeatVec;\n const cv::Mat &Descriptors2 = pKF2->framePtr->mDescriptors;\n \n vector rotHist[HISTO_LENGTH];\n for(int i=0;ifirst == f2it->first)\n {\n for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];\n \n \n if (matches12[idx1]>=0) {\n nmatches++;\n assert(0);\n continue;\n }\n \n if (!pKF1->mvLocalMapPoints[idx1].isEstimated) {\n continue;\n }\n \n const cv::Mat &d1 = Descriptors1.row(idx1);\n \n int bestDist1=256;\n int bestIdx2 =-1 ;\n int bestDist2=256;\n \n for(size_t i2=0, iend2=f2it->second.size(); i2second[i2];\n \n if (matches21[idx2]>=0) {\n continue;\n }\n \n const cv::Mat &d2 = Descriptors2.row(idx2);\n \n int dist = DescriptorDistance(d1,d2);\n \n if(dist(bestDist1)(bestDist2)){\n \n matches12[idx1]=bestIdx2;\n matches21[bestIdx2]=idx1;\n \n if(mbCheckOrientation){\n float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;\n if(rot<0.0)\n rot+=360.0f;\n int bin = round(rot*factor);\n if(bin==HISTO_LENGTH)\n bin=0;\n assert(bin>=0 && binfirst < f2it->first){\n f1it = vFeatVec1.lower_bound(f2it->first);\n }else{\n f2it = vFeatVec2.lower_bound(f1it->first);\n }\n }\n \n if(mbCheckOrientation)\n {\n int ind1=-1;\n int ind2=-1;\n int ind3=-1;\n \n ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);\n \n for(int i=0; i &matches12,vector &matches21){\n \n const vector &vKeysUn1 = pKF1->framePtr->mvKeysUn;\n const DBoW2::FeatureVector &vFeatVec1 = pKF1->framePtr->mFeatVec;\n const cv::Mat &Descriptors1 = pKF1->framePtr->mDescriptors;\n \n const vector &vKeysUn2 = pKF2->framePtr->mvKeysUn;\n const DBoW2::FeatureVector &vFeatVec2 = pKF2->framePtr->mFeatVec;\n const cv::Mat &Descriptors2 = pKF2->framePtr->mDescriptors;\n \n vector rotHist[HISTO_LENGTH];\n for(int i=0;ifirst == f2it->first)\n {\n for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];\n \n if (matches12[idx1]>=0) {\n nmatches++;\n continue;\n }\n \n if (!pKF1->mvLocalMapPoints[idx1].isEstimated) {\n continue;\n }\n \n const cv::Mat &d1 = Descriptors1.row(idx1);\n \n int bestDist1=256;\n int bestIdx2 =-1 ;\n int bestDist2=256;\n \n for(size_t i2=0, iend2=f2it->second.size(); i2second[i2];\n \n if (matches21[idx2]>=0) {\n continue;\n }\n \n const cv::Mat &d2 = Descriptors2.row(idx2);\n \n int dist = DescriptorDistance(d1,d2);\n \n if(dist(bestDist1)(bestDist2)){\n \n matches12[idx1]=bestIdx2;\n matches21[bestIdx2]=idx1;\n \n if(mbCheckOrientation){\n float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;\n if(rot<0.0)\n rot+=360.0f;\n int bin = round(rot*factor);\n if(bin==HISTO_LENGTH)\n bin=0;\n assert(bin>=0 && binfirst < f2it->first){\n f1it = vFeatVec1.lower_bound(f2it->first);\n }else{\n f2it = vFeatVec2.lower_bound(f1it->first);\n }\n }\n \n if(mbCheckOrientation)\n {\n int ind1=-1;\n int ind2=-1;\n int ind3=-1;\n \n ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);\n \n for(int i=0; i* histo, const int L, int &ind1, int &ind2, int &ind3)\n{\n int max1=0;\n int max2=0;\n int max3=0;\n\n for(int i=0; imax1)\n {\n max3=max2;\n max2=max1;\n max1=s;\n ind3=ind2;\n ind2=ind1;\n ind1=i;\n }\n else if(s>max2)\n {\n max3=max2;\n max2=s;\n ind3=ind2;\n ind2=i;\n }\n else if(s>max3)\n {\n max3=s;\n ind3=i;\n }\n }\n\n if(max2<0.1f*(float)max1)\n {\n ind2=-1;\n ind3=-1;\n }\n else if(max3<0.1f*(float)max1)\n {\n ind3=-1;\n }\n}\n\n\n// Bit set count operation from\n// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel\nint ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)\n{\n const int *pa = a.ptr();\n const int *pb = b.ptr();\n\n int dist=0;\n\n for(int i=0; i<8; i++, pa++, pb++)\n {\n unsigned int v = *pa ^ *pb;\n v = v - ((v >> 1) & 0x55555555);\n v = (v & 0x33333333) + ((v >> 2) & 0x33333333);\n dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;\n }\n\n return dist;\n}\n\n} //namespace ORB_SLAM\n" }, { "path": "GSLAM/ORBmatcher.h", "content": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)\n* For more information see \n*\n* ORB-SLAM2 is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* ORB-SLAM2 is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with ORB-SLAM2. If not, see .\n*/\n\n\n#ifndef ORBMATCHER_H\n#define ORBMATCHER_H\n\n#include \n#include \"opencv2/core/core.hpp\"\n#include \"opencv2/features2d/features2d.hpp\"\n\n#include\"KeyFrame.h\"\n#include\"Frame.h\"\n\n\nnamespace GSLAM\n{\n\n class ORBmatcher{\n public:\n\n ORBmatcher(float nnratio=0.6, bool checkOri=true);\n\n // Computes the Hamming distance between two ORB descriptors\n static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);\n \n int SearchByBoW(KeyFrame *keyFrame1,KeyFrame *keyFrame2,std::vector &matches12,std::vector &matches21);\n \n int SearchByBoWLoop(KeyFrame *keyFrame1,KeyFrame *keyFrame2,std::vector &matches12,std::vector &matches21);\n \n int SearchByTracking(KeyFrame *keyFrame1,KeyFrame *keyFrame2,\n std::vector &matches12,std::vector &matches21,int ORBdist);\n \n int SearchByProjection(KeyFrame *keyFrame1,KeyFrame *keyFrame2,Transform transform,\n std::vector &matches12,std::vector &matches21,\n const float th,const int ORBdist);\n \n int SearchByProjectionLoop(KeyFrame *keyFrame1,KeyFrame *keyFrame2,Transform transform,\n std::vector &matches12,std::vector &matches21,\n const float th,const int ORBdist);\n \n int SearchByRotation(Frame* frame1,Frame* frame2,const cv::Mat& rotation,\n std::vector& pts1,std::vector& pts2,int ORBdist);\n \n //backward projection;\n int RefineMatchByProjection(KeyFrame *keyFrame1,KeyFrame *keyFrame2,Transform transform,\n std::vector &matches12,std::vector &matches21,\n const float th,const int ORBdist);\n \n \n Eigen::Matrix3d K;\n double projErrorThreshold;\n \n public:\n static const int TH_LOW;\n static const int TH_HIGH;\n static const int HISTO_LENGTH;\n\n\n protected:\n\n bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF);\n float RadiusByViewingCos(const float &viewCos);\n void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3);\n float mfNNratio;\n bool mbCheckOrientation;\n};\n\n}// namespace ORB_SLAM\n\n#endif // ORBMATCHER_H\n" }, { "path": "GSLAM/RelativeMotion.hpp", "content": "//\n// RelativeMotion.hpp\n// GSLAM\n//\n// Created by Chaos on 2/17/17.\n// Copyright © 2017 ctang. All rights reserved.\n//\n\n#ifndef RelativeMotion_h\n#define RelativeMotion_h\n\n#include \"ceres/rotation.h\"\n#include \"ceres/ceres.h\"\n\nstruct RotationTranslation{\n \n RotationTranslation(const double _x1,const double _y1,const double _z1,\n const double _x2,const double _y2,const double _z2):\n x1(_x1),y1(_y1),z1(_z1),x2(_x2),y2(_y2),z2(_z2){\n }\n \n template \n bool operator()(const T* const camera,\n T* residuals) const {\n \n T rotated1[3];\n T p1[3]={(T)x1,(T)y1,(T)z1};\n T p2[3]={(T)x2,(T)y2,(T)z2};\n ceres::AngleAxisRotatePoint(camera,p1,rotated1);\n T norm=camera[3]*camera[3]+camera[4]*camera[4]+camera[5]*camera[5];\n \n if(sqrt(norm)>=(T)std::numeric_limits::min()){//translation is not zero\n \n T axis[3];\n ceres::CrossProduct(rotated1,&camera[3],axis);\n T D=-(axis[0]*rotated1[0]+axis[1]*rotated1[1]+axis[2]*rotated1[2]);\n \n norm=axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2];\n T t=(axis[0]*p2[0]+axis[1]*p2[1]+axis[2]*p2[2]+D)/norm;\n \n rotated1[0]=p2[0]+axis[0]*t;\n rotated1[1]=p2[1]+axis[1]*t;\n rotated1[2]=p2[2]+axis[2]*t;\n }\n \n norm=sqrt(rotated1[0]*rotated1[0]+rotated1[1]*rotated1[1]+rotated1[2]*rotated1[2]);\n residuals[0]=rotated1[0]/norm-p2[0];\n residuals[1]=rotated1[1]/norm-p2[1];\n residuals[2]=rotated1[2]/norm-p2[2];\n return true;\n }\n \n static ceres::CostFunction* Create(const double x1,\n const double y1,\n const double z1,\n const double x2,\n const double y2,\n const double z2) {\n \n return (new ceres::AutoDiffCostFunction(new RotationTranslation(x1,y1,z1,x2,y2,z2)));\n \n };\n \n double x1;\n double y1;\n double z1;\n double x2;\n double y2;\n double z2;\n};\n\n\n\nstruct Rotation{\n \n Rotation(const double _x1,const double _y1,const double _z1,\n const double _x2,const double _y2,const double _z2):\n x1(_x1),y1(_y1),z1(_z1),x2(_x2),y2(_y2),z2(_z2){\n }\n \n template \n bool operator()(const T* const camera,\n T* residuals) const {\n \n T rotated1[3];\n T p1[3]={(T)x1,(T)y1,(T)z1};\n T p2[3]={(T)x2,(T)y2,(T)z2};\n ceres::AngleAxisRotatePoint(camera,p1,rotated1);\n residuals[0]=rotated1[0]-p2[0];\n residuals[1]=rotated1[1]-p2[1];\n residuals[2]=rotated1[2]-p2[2];\n return true;\n }\n \n static ceres::CostFunction* Create(const double x1,\n const double y1,\n const double z1,\n const double x2,\n const double y2,\n const double z2) {\n \n return (new ceres::AutoDiffCostFunction(new Rotation(x1,y1,z1,x2,y2,z2)));\n \n };\n \n double x1;\n double y1;\n double z1;\n double x2;\n double y2;\n double z2;\n};\n\nEigen::Vector3d estimateRelativeTranslation(const std::vector &pts1,\n const std::vector &pts2){\n \n int num_point=pts1.size();\n std::vector norms(num_point);\n Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);\n Eigen::Vector3d preResult,curResult;\n for(int i=0;i svd(NtN,Eigen::ComputeFullV);\n Eigen::Matrix3d V=svd.matrixV();\n curResult=V.col(2);\n return curResult;\n}\n\n\nvoid preSolve(const std::vector &pts1,\n const std::vector &pts2,\n const Eigen::Matrix3d& rotation,\n Eigen::Vector3d& translation){\n \n std::vector _pts1(pts1.size()),_pts2(pts2.size());\n for(int p=0;p _pts1(pts1.size()),_pts2(pts2.size());\n \n for(int p=0;p &pts1,\n const std::vector &pts2,\n Eigen::Matrix3d& rotation,\n Eigen::Vector3d& translation){\n \n \n assert(pts1.size()==pts2.size());\n preSolve(pts1,pts2,rotation,translation);\n \n double motion[6]={0};\n ceres::RotationMatrixToAngleAxis(rotation.data(),&motion[0]);\n motion[3]=translation(0);\n motion[4]=translation(1);\n motion[5]=translation(2);\n \n \n ceres::Problem problem;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(lossThreshold);\n \n for(int i=0;i &pts1,\n const std::vector &pts2,\n std::vector& ratios,\n Eigen::Matrix3d& rotation,\n Eigen::Vector3d& translation){\n \n \n assert(pts1.size()==pts2.size());\n preSolve(pts1,pts2,rotation,translation);\n \n double motion[6]={0};\n ceres::RotationMatrixToAngleAxis(rotation.data(),&motion[0]);\n motion[3]=translation(0);\n motion[4]=translation(1);\n motion[5]=translation(2);\n \n \n ceres::Problem problem;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(lossThreshold);\n \n for(int i=0;i\n bool operator()(const T* const camera,\n const T* const radius,\n T* residuals) const {\n \n T p[3];\n \n T point[3]={(T)x1,(T)y1,(T)z1};\n T norm=sqrt(camera[3]*camera[3]+camera[4]*camera[4]+camera[5]*camera[5]);\n ceres::AngleAxisRotatePoint(camera,point,p);\n p[0]+=(*radius)*(camera[3]-p[0]);\n p[1]+=(*radius)*(camera[4]-p[1]);\n p[2]+=(*radius)*(camera[5]-p[2]);\n \n norm=sqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]);\n residuals[0]=p[0]/norm-(T)x2;\n residuals[1]=p[1]/norm-(T)y2;\n residuals[2]=p[2]/norm-(T)z2;\n return true;\n }\n \n static ceres::CostFunction* Create(const double x1,\n const double y1,\n const double z1,\n const double x2,\n const double y2,\n const double z2) {\n \n return (new ceres::AutoDiffCostFunction(new Transform(x1,y1,z1,x2,y2,z2)));\n \n };\n \n double x1;\n double y1;\n double z1;\n double x2;\n double y2;\n double z2;\n};\n\nvoid estimateRotationTranslation2(const double lossThreshold,\n const std::vector &pts1,\n const std::vector &pts2,\n Eigen::Matrix3d& rotation,\n Eigen::Vector3d& translation){\n \n double motion[6]={0};\n ceres::RotationMatrixToAngleAxis(rotation.data(),&motion[0]);\n motion[3]=translation(0);\n motion[4]=translation(1);\n motion[5]=translation(2);\n \n ceres::Problem problem;\n ceres::LossFunction* loss_function = NULL;\n std::vector ratios(pts1.size(),0.0);\n \n \n for(int i=0;i\n#include \n\n\nnamespace GSLAM{\n \n System::System(const string &strVocFile, const string &strSettingsFile){\n \n // Output welcome message\n cout << endl <<\n \"GSLAM Copyright (C) 2014-2016 Chengzhou Tang, Simon Fraser University.\" << endl <<\n \"This program comes with ABSOLUTELY NO WARRANTY;\" << endl <<\n \"This is free software, and you are welcome to redistribute it\" << endl <<\n \"under certain conditions. See LICENSE.txt.\" << endl << endl;\n \n \n //Check settings file\n cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);\n if(!fsSettings.isOpened())\n {\n cerr << \"Failed to open settings file at: \" << strSettingsFile << endl;\n exit(-1);\n }\n \n //Load ORB Vocabulary\n cout << endl << \"Loading ORB Vocabulary. This could take a while...\" << endl;\n \n mpVocabulary = new ORBVocabulary();\n \n bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);\n if(!bVocLoad)\n {\n cerr << \"Wrong path to vocabulary. \" << endl;\n cerr << \"Falied to open at: \" << strVocFile << endl;\n exit(-1);\n }\n cout << \"Vocabulary loaded!\" << endl << endl;\n \n mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);\n cout << \"Keyframe Database created!\" << endl << endl;\n \n \n //loading parameter\n cameraSettings.fx=fsSettings[\"Camera.fx\"];\n cameraSettings.fy=fsSettings[\"Camera.fy\"];\n cameraSettings.ox=fsSettings[\"Camera.ox\"];\n cameraSettings.oy=fsSettings[\"Camera.oy\"];\n \n mK=cv::Mat::zeros(3,3,CV_64FC1);\n mK.at(0,0) = cameraSettings.fx;\n mK.at(1,1) = cameraSettings.fy;\n mK.at(0,2) = cameraSettings.ox;\n mK.at(1,2) = cameraSettings.oy;\n \n \n cameraSettings.k1=fsSettings[\"Camera.k1\"];\n cameraSettings.k2=fsSettings[\"Camera.k2\"];\n cameraSettings.p1=fsSettings[\"Camera.p1\"];\n cameraSettings.p2=fsSettings[\"Camera.p2\"];\n cameraSettings.k3=fsSettings[\"Camera.k3\"];\n\n mDistCoef=cv::Mat(4,1,CV_32F);\n mDistCoef.at(0) = fsSettings[\"Camera.k1\"];\n mDistCoef.at(1) = fsSettings[\"Camera.k2\"];\n mDistCoef.at(2) = fsSettings[\"Camera.p1\"];\n mDistCoef.at(3) = fsSettings[\"Camera.p2\"];\n \n const float k3 = fsSettings[\"Camera.k3\"];\n if(k3!=0){\n mDistCoef.resize(5);\n mDistCoef.at(4) = k3;\n }\n \n int nFeatures = fsSettings[\"ORBextractor.nFeatures\"];\n float fScaleFactor= fsSettings[\"ORBextractor.scaleFactor\"];\n int nLevels = fsSettings[\"ORBextractor.nLevels\"];\n int fIniThFAST = fsSettings[\"ORBextractor.iniThFAST\"];\n int fMinThFAST = fsSettings[\"ORBextractor.minThFAST\"];\n \n mpORBExtractor= new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);\n mpORBExtractorFrame= new ORBextractor(1000,1.2,2,fIniThFAST,fMinThFAST);\n \n //orbMatcher=new ORBmatcher(0.8,false);\n trackingContext=KLTCreateTrackingContext();\n \n \n trackingContext->window_width=fsSettings[\"KLT.window_width\"];\n trackingContext->window_height=fsSettings[\"KLT.window_height\"];\n \n trackingContext->nPyramidLevels=fsSettings[\"KLT.nPyramidLevels\"];\n trackingContext->borderx=fsSettings[\"KLT.borderx\"];\n trackingContext->bordery=fsSettings[\"KLT.bordery\"];\n \n trackingContext->min_eigenvalue=fsSettings[\"KLT.min_eigenvalue\"];\n trackingContext->mindist=fsSettings[\"KLT.mindist\"];\n \n mScaleFactor=std::pow(2,trackingContext->nPyramidLevels-1);\n mK=mK/mScaleFactor;\n mK.at(2,2)=1.0;\n mKInv=mK.inv();\n \n \n featureList=KLTCreateFeatureList(trackingContext,2000);\n \n \n frameId=0;\n \n K=Eigen::Matrix3d::Zero();\n K(0,0)=cameraSettings.fx;\n K(1,1)=cameraSettings.fy;\n K(2,2)=1.0;\n K(0,2)=cameraSettings.ox;\n K(1,2)=cameraSettings.oy;\n invK=K.inverse();\n \n cvInvK=cv::Mat(3,3,CV_64FC1);\n for (int r1=0;r1<3;r1++) {\n for (int r2=0;r2<3;r2++) {\n cvInvK.at(r1,r2)=invK(r1,r2);\n }\n }\n \n \n imu.ts=fsSettings[\"Camera.ts\"];\n cameraSettings.td=fsSettings[\"Camera.td\"];\n \n slamSettings.medViewAngle=fsSettings[\"SLAM.medViewAngle\"];\n slamSettings.medViewAngle=std::cos((slamSettings.medViewAngle/180.0)*CV_PI);\n slamSettings.minViewAngle=fsSettings[\"SLAM.minViewAngle\"];\n slamSettings.minViewAngle=std::cos((slamSettings.minViewAngle/180.0)*CV_PI);\n \n \n slamSettings.requireNewKeyFrameCount=fsSettings[\"SLAM.requireNewKeyFrameCount\"];\n \n keyFrameConnector.matcherByTracking=new ORBmatcher(0.8,false);\n keyFrameConnector.matcherByProjection=new ORBmatcher(0.8,false);\n keyFrameConnector.matcherByBoW=new ORBmatcher(0.75,true);\n \n keyFrameConnector.matcherByTracking->K=K/mScaleFactor;\n keyFrameConnector.matcherByTracking->K(2,2)=1.0;\n \n keyFrameConnector.matcherByProjection->K=keyFrameConnector.matcherByTracking->K;\n \n \n lightFramePre=NULL;\n lightFrameCur=NULL;\n \n frameStart=fsSettings[\"SLAM.startFrame\"];\n frameEnd=fsSettings[\"SLAM.endFrame\"];\n }\n \n void processKeyFrame(System& system,KeyFrame& keyFrame){\n \n LocalFactorization localFactorization;\n localFactorization.process(&keyFrame);\n //localFactorization.iterativeRefine(keyFrame);\n \n#ifdef DEBUG\n //sprintf(name,\"/Users/chaos/Desktop/debug/init_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n#endif\n \n LocalBundleAdjustment localBundleAdjustment;\n localBundleAdjustment.projErrorThres=0.005;\n localBundleAdjustment.viewAngleThres=system.slamSettings.minViewAngle;\n //printf(\"ba\\n\");\n \n localBundleAdjustment.bundleAdjust(&keyFrame);\n \n \n //sprintf(name,\"/Users/chaos/Desktop/debug/ba0_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n \n localBundleAdjustment.triangulate2(&keyFrame);\n \n //sprintf(name,\"/Users/chaos/Desktop/debug/tra_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n \n \n localBundleAdjustment.bundleAdjust(&keyFrame);\n //localBundleAdjustment.refinePoints(&keyFrame);\n \n //keyFrame->visualize();\n \n //localBundleAdjustment.estimateCloseFrames(keyFrame);\n //getchar();\n#ifdef DEBUG\n char name[200];\n //sprintf(name,\"/Users/chaos/Desktop/syndata/data_%d.txt\",keyFrame.frameId);\n //keyFrame.saveData2(name);\n sprintf(name,\"/Users/chaos/Desktop/debug/data_%d.ply\",keyFrame.frameId);\n keyFrame.savePly(name);\n#endif\n if(keyFrame.prevKeyFramePtr!=NULL){\n if (keyFrame.prevKeyFramePtr->baThread.joinable()) {\n keyFrame.prevKeyFramePtr->baThread.join();\n }\n \n system.keyFrameConnector.mpORBVocabulary=system.mpVocabulary;\n system.keyFrameConnector.keyFrameDatabase=system.mpKeyFrameDatabase;\n system.keyFrameConnector.connectionThreshold=80;\n system.keyFrameConnector.connectKeyFrame(keyFrame.prevKeyFramePtr,&keyFrame);\n }\n \n system.mpKeyFrameDatabase->add(&keyFrame);\n system.globalReconstruction.addNewKeyFrame(&keyFrame);\n }\n \n class NormalizeFrameInvoker{\n private:\n KeyFrame* keyFrame;\n ImageGrids* grids;\n KLT_FeatureList* featureList;\n std::vector* rotations;\n cv::Mat* cvInvK;\n public:\n NormalizeFrameInvoker(KeyFrame* _keyFrame,\n ImageGrids* _grids,\n KLT_FeatureList* _featureList,\n std::vector* _rotations,\n cv::Mat* _cvInvK){\n keyFrame=_keyFrame;\n grids=_grids;\n featureList=_featureList;\n rotations=_rotations;\n cvInvK=_cvInvK;\n }\n void operator ()(const tbb::blocked_range& range) const;\n };\n \n \n void NormalizeFrameInvoker::operator()(const tbb::blocked_range& range) const{\n \n for (int i=range.begin();ifeature[i]->val==KLT_TRACKED) {\n \n cv::Mat dst;\n grids->rotateAndNormalizePoint(cv::Point2f((*featureList)->feature[i]->x,\n (*featureList)->feature[i]->y),\n dst,*rotations,*cvInvK);\n \n (*featureList)->feature[i]->norm(0)=dst.at(0);\n (*featureList)->feature[i]->norm(1)=dst.at(1);\n (*featureList)->feature[i]->norm(2)=dst.at(2);\n (*featureList)->feature[i]->vec=(*featureList)->feature[i]->norm\n /(*featureList)->feature[i]->norm(2);\n \n keyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f((*featureList)->feature[i]->x,\n (*featureList)->feature[i]->y));\n }\n }\n }\n \n class NormalizeKeyFrameInvoker{\n \n private:\n KeyFrame* keyFrame;\n ImageGrids* grids;\n KLT_FeatureList* featureList;\n std::vector* rotations;\n cv::Mat* cvInvK;\n cv::Mat* colorImage;\n public:\n NormalizeKeyFrameInvoker(KeyFrame* _keyFrame,\n ImageGrids* _grids,\n KLT_FeatureList* _featureList,\n std::vector* _rotations,\n cv::Mat* _cvInvK,\n cv::Mat* _colorImage){\n keyFrame=_keyFrame;\n grids=_grids;\n featureList=_featureList;\n rotations=_rotations;\n cvInvK=_cvInvK;\n colorImage=_colorImage;\n }\n\n \n void operator ()(const tbb::blocked_range& range) const;\n };\n \n void NormalizeKeyFrameInvoker::operator()(const tbb::blocked_range& range) const{\n for(int i=range.begin();irotateAndNormalizePoint(cv::Point2f((*featureList)->feature[i]->x,(*featureList)->feature[i]->y),\n dst,*rotations,*cvInvK);\n \n keyFrame->mvLocalMapPoints[i].norm(0)=dst.at(0);\n keyFrame->mvLocalMapPoints[i].norm(1)=dst.at(1);\n keyFrame->mvLocalMapPoints[i].norm(2)=dst.at(2);\n keyFrame->mvLocalMapPoints[i].vec=keyFrame->mvLocalMapPoints[i].norm\n /keyFrame->mvLocalMapPoints[i].norm(2);\n \n cv::Vec3b color=colorImage->at(round((*featureList)->feature[i]->y/2),\n round((*featureList)->feature[i]->x/2));\n \n keyFrame->mvLocalMapPoints[i].color[0]=color.val[0];\n keyFrame->mvLocalMapPoints[i].color[1]=color.val[1];\n keyFrame->mvLocalMapPoints[i].color[2]=color.val[2];\n keyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f((*featureList)->feature[i]->x,\n (*featureList)->feature[i]->y));\n }\n }\n \n \n \n Transform System::Track(cv::Mat &im, const double ×tamp,const int outIndex){\n Transform pose;\n \n#ifdef DEBUG\n \n static vector colors;\n static vector keyPoints;\n\n trackingContext->min_eigenvalue=100;\n trackingContext->mindist=10;\n \n trackingContext->writeInternalImages = FALSE;\n trackingContext->affineConsistencyCheck = FALSE;\n trackingContext->lighting_insensitive=FALSE;\n \n trackingContext->window_width=21;\n trackingContext->window_height=21;\n trackingContext->nPyramidLevels=2;\n trackingContext->borderx=50;\n trackingContext->bordery=50;\n \n#endif\n \n if (Frame::mbInitialComputations) {\n //google::InitGoogleLogging(\"test\");\n //for get rotation need to change!\n int frameWidth=im.cols,frameHeight=im.rows;\n frameSize=cv::Size(frameWidth,frameHeight);\n cv::Size gridSize(20,20),sizeByGrid;\n sizeByGrid.width=frameWidth/gridSize.width;\n sizeByGrid.height=frameHeight/gridSize.height;\n sizeByVertex=sizeByGrid+cv::Size(1,1);\n grids.initialize(gridSize,sizeByGrid,sizeByVertex);\n rotations.resize(sizeByVertex.height);\n \n frameIndex=0;\n globalRotations.clear();\n frameStamp=timestamp+cameraSettings.td;\n \n imu.getIntraFrameRotation(rotations,grids.originGridVertices,frameSize,sizeByVertex,frameIndex,frameStamp);\n rotations[rotations.size()-1].copyTo(rotation);\n globalRotations.push_back(cv::Mat::eye(3,3,CV_64FC1));\n \n //create klt frame\n pyramidBuffers.initialize();\n computePyramid(trackingContext,im,*pyramidBuffers.ptrs[1]);\n \n pyramidBuffers.preloadThread=std::thread(computePyramid,\n std::ref(trackingContext),\n std::ref(*preloadImage),\n std::ref(*pyramidBuffers.ptrs[2]));\n //char name[200];\n //printf(name,\"/Users/ctang/edge.png\");\n /*cv::Mat normx;\n cv::Mat normy;\n cv::Mat normxy;\n \n cv::multiply(pyramid1[1],pyramid1[1],normx);\n cv::multiply(pyramid1[2],pyramid1[2],normy);\n\n cv::sqrt(normx+normy,normxy);\n cv::imwrite(\"/Users/ctang/edge.png\",normxy);getchar();*/\n /*cv::Mat gx,gy,gxx,gyy,gxy,sumGxx,sumGyy,sumGxy;\n cv::multiply(pyramid1[1],pyramid1[1],gxx);\n cv::multiply(pyramid1[2],pyramid1[2],gyy);\n cv::multiply(pyramid1[1],pyramid1[2],gxy);\n \n cv::integral(gxx,sumGxx,CV_32F);\n cv::integral(gyy,sumGyy,CV_32F);\n cv::integral(gxy,sumGxy,CV_32F);\n \n KLTSelectGoodFeatures(trackingContext,sumGxx,sumGyy,sumGxy,featureList);*/\n\n /*pyramid1Ptr=&pyramid1;\n pyramid2Ptr=&pyramid2;\n pyramid3Ptr=&pyramid3;\n pyramidTmpPtr=NULL;*/\n \n \n \n \n //create orb frame and keyframe\n cv::Mat ucharImage;\n //pyramid1[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1);\n (*pyramidBuffers.ptrs[1])[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1);\n \n Frame *framePtr=new Frame(ucharImage,timestamp,mpORBExtractor,mpVocabulary,mK,mDistCoef);\n KeyFrame *keyFramePtr=new KeyFrame(framePtr,mpKeyFrameDatabase,invK,mScaleFactor);\n keyFramePtr->frameId=frameId;\n keyFramePtr->outId=outIndex;\n \n \n for(int i=0;imvLocalMapPoints.size();i++){\n featureList->feature[i]->x=keyFramePtr->framePtr->mvKeys[i].pt.x*mScaleFactor;\n featureList->feature[i]->y=keyFramePtr->framePtr->mvKeys[i].pt.y*mScaleFactor;\n featureList->feature[i]->val=KLT_TRACKED;\n }\n featureList->nFeatures=keyFramePtr->mvLocalMapPoints.size();\n frontKeyFrame=keyFramePtr;\n preKeyFrame=NULL;\n \n /*for(int i=0;imvLocalMapPoints.size();i++){\n \n cv::Mat dst;\n grids.rotateAndNormalizePoint(cv::Point2f(featureList->feature[i]->x,featureList->feature[i]->y),\n dst,rotations,cvInvK);\n \n frontKeyFrame->mvLocalMapPoints[i].norm(0)=dst.at(0);\n frontKeyFrame->mvLocalMapPoints[i].norm(1)=dst.at(1);\n frontKeyFrame->mvLocalMapPoints[i].norm(2)=dst.at(2);\n frontKeyFrame->mvLocalMapPoints[i].vec=frontKeyFrame->mvLocalMapPoints[i].norm\n /frontKeyFrame->mvLocalMapPoints[i].norm(2);\n \n cv::Vec3b color=colorImage->at(round(featureList->feature[i]->y/2),round(featureList->feature[i]->x/2));\n frontKeyFrame->mvLocalMapPoints[i].color[0]=color.val[0];\n frontKeyFrame->mvLocalMapPoints[i].color[1]=color.val[1];\n frontKeyFrame->mvLocalMapPoints[i].color[2]=color.val[2];\n frontKeyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f(featureList->feature[i]->x,\n featureList->feature[i]->y));\n }*/\n \n tbb::parallel_for(tbb::blocked_range(0,frontKeyFrame->mvLocalMapPoints.size()),NormalizeKeyFrameInvoker(frontKeyFrame,&grids,&featureList,&rotations,&cvInvK,colorImage));\n \n //lightFramePre=new Frame(ucharImage,timestamp,mpORBExtractorFrame,mpVocabulary,mK,mDistCoef);\n \n }else{\n \n \n /*\n cv::Mat ucharImage;\n (*pyramid2Ptr)[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1);\n lightFrameCur=new Frame(ucharImage,timestamp,mpORBExtractorFrame,mpVocabulary,mK,mDistCoef);\n */\n \n \n frameStamp=timestamp+cameraSettings.td;\n imu.getInterFrameRotation(rotation,frameIndex,preFrameStamp,frameStamp);\n globalRotations.push_back(rotation*globalRotations[frameId-1]);\n \n /*\n std::vector pts1,pts2;\n cv::Mat transform=mK*rotation*mKInv;\n orbMatcher->SearchByRotation(lightFramePre,lightFrameCur,transform,pts1,pts2,48);\n std::vector intensity1(pts1.size()),intensity2(pts2.size());\n for (int p=0;pnPyramidLevels-1)].at((int)pts1[p].y,\n (int)pts1[p].x);\n \n intensity2[p]=(*pyramid2Ptr)[3*(trackingContext->nPyramidLevels-1)].at((int)pts2[p].y,\n (int)pts2[p].x);\n }\n compensateLightingAndMotion(pts1,pts2,intensity1,intensity2);\n */\n \n /*for(int i=0;i<9;i++){\n std::cout<(i)<nPyramidLevels-1)],debugImage,CV_GRAY2BGR);\n \n for (int p=0;pnFeatures;i++) {\n if (featureList->feature[i]->val==KLT_TRACKED) {\n \n cv::Mat dst;\n grids.rotateAndNormalizePoint(cv::Point2f(featureList->feature[i]->x,\n featureList->feature[i]->y),\n dst,rotations,cvInvK);\n \n featureList->feature[i]->norm(0)=dst.at(0);\n featureList->feature[i]->norm(1)=dst.at(1);\n featureList->feature[i]->norm(2)=dst.at(2);\n featureList->feature[i]->vec=featureList->feature[i]->norm\n /featureList->feature[i]->norm(2);\n \n frontKeyFrame->mvLocalMapPoints[i].tracked.push_back(cv::Point2f(featureList->feature[i]->x,\n featureList->feature[i]->y));\n }\n }*/\n \n tbb::parallel_for(tbb::blocked_range(0,featureList->nFeatures),\n NormalizeFrameInvoker(frontKeyFrame,&grids,&featureList,&rotations,&cvInvK));\n \n \n KeyFrame* keyFrame=frontKeyFrame;\n int pVectorCount=0,fullTrackCount=0,inlierCount=0;\n std::vector isValid(keyFrame->mvLocalMapPoints.size());\n for (int i=0;imvLocalMapPoints.size();i++) {\n isValid[i]=keyFrame->mvLocalMapPoints[i].isFullTrack;\n }\n \n RelativeOutlierRejection outlierRejection;\n outlierRejection.K=K;\n for (int i1=0;i1<3;i1++) {\n for (int i2=0;i2<3;i2++) {\n outlierRejection.rotation(i1,i2)=globalRotations[globalRotations.size()-1].at(i1,i2);\n }\n }\n \n outlierRejection.rotation=outlierRejection.rotation*keyFrame->pose.rotation.transpose();\n outlierRejection.theshold=10.0;\n outlierRejection.prob=0.98;\n outlierRejection.medViewAngle=slamSettings.medViewAngle;\n outlierRejection.minViewAngle=slamSettings.minViewAngle;\n outlierRejection.isValid=&isValid;\n inlierCount=outlierRejection.relativeRansac(keyFrame,featureList);\n \n RelativePoseEstimation relativeEstimation;\n std::vector pVectors;\n \n /*static Eigen::Matrix3d prevRotation;\n static Eigen::Vector3d prevTranslation;\n \n if (keyFrame->mvLocalFrames.size()==0) {\n outlierRejection.rotation=Eigen::Matrix3d::Identity();\n prevTranslation=Eigen::Vector3d::Zero();\n }else{\n outlierRejection.rotation=prevRotation;\n }*/\n \n if(inlierCount>0){\n relativeEstimation.isValid=&isValid;\n \n relativeEstimation.rotation=outlierRejection.rotation;\n //relativeEstimation.translation=prevTranslation;\n \n relativeEstimation.rotationIsKnown=true;\n relativeEstimation.medViewAngle=slamSettings.medViewAngle;\n relativeEstimation.minViewAngle=slamSettings.minViewAngle;\n pVectorCount=relativeEstimation.estimateRelativePose(keyFrame,featureList,pVectors);\n }\n //prevRotation=relativeEstimation.rotation;\n //prevTranslation=relativeEstimation.translation;\n \n \n bool requestNewKeyFrame=false;\n bool isLastKeyFrame=false;\n \n if (pVectorCount>0) {\n \n fullTrackCount=keyFrame->appendRelativeEstimation(frameId,NULL,\n relativeEstimation.rotation,\n relativeEstimation.translation,\n featureList,pVectors);\n \n requestNewKeyFrame=fullTrackCountmvLocalFrames.size()>15;\n //requestNewKeyFrame=(requestNewKeyFrame||keyFrame->mvLocalFrames.size()>40);\n isLastKeyFrame=true;\n }\n \n \n if(requestNewKeyFrame){\n \n printf(\"new %d old %d\\n\",frameId,frontKeyFrame->frameId);\n char name[200];\n \n /*LocalFactorization localFactorization;\n localFactorization.process(keyFrame);\n //localFactorization.iterativeRefine(keyFrame);\n \n#ifdef DEBUG\n //sprintf(name,\"/Users/chaos/Desktop/debug/init_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n#endif\n \n LocalBundleAdjustment localBundleAdjustment;\n localBundleAdjustment.projErrorThres=0.005;\n localBundleAdjustment.viewAngleThres=slamSettings.minViewAngle;\n //printf(\"ba\\n\");\n \n localBundleAdjustment.bundleAdjust(keyFrame);\n \n \n //sprintf(name,\"/Users/chaos/Desktop/debug/ba0_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n \n localBundleAdjustment.triangulate2(keyFrame);\n \n //sprintf(name,\"/Users/chaos/Desktop/debug/tra_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n\n \n //localBundleAdjustment.bundleAdjust(keyFrame);\n localBundleAdjustment.refinePoints(keyFrame);\n \n //keyFrame->visualize();\n \n //localBundleAdjustment.estimateCloseFrames(keyFrame);\n //getchar();\n#ifdef DEBUG\n //sprintf(name,\"/Users/chaos/Desktop/debug/ba1_%d_%d.ply\",frontKeyFrame->frameId,frameId);\n //keyFrame->savePly(name);\n //getchar();\n#endif\n\n \n if(preKeyFrame!=NULL){\n keyFrameConnector.mpORBVocabulary=mpVocabulary;\n keyFrameConnector.keyFrameDatabase=mpKeyFrameDatabase;\n keyFrameConnector.connectionThreshold=80;\n keyFrameConnector.connectKeyFrame(preKeyFrame,keyFrame);\n }*/\n keyFrame->prevKeyFramePtr=preKeyFrame;\n keyFrame->baThread=std::thread(processKeyFrame,std::ref(*this),std::ref(*keyFrame));\n //processKeyFrame(*this,*keyFrame);\n \n \n //mpKeyFrameDatabase->add(keyFrame);\n //globalReconstruction.addNewKeyFrame(keyFrame);\n \n \n cv::Mat ucharImage;\n (*pyramid2Ptr)[3*(trackingContext->nPyramidLevels-1)].convertTo(ucharImage,CV_8UC1);\n Frame *framePtr=new Frame(ucharImage,timestamp,mpORBExtractor,mpVocabulary,mK,mDistCoef);\n KeyFrame *keyFramePtr=new KeyFrame(framePtr,mpKeyFrameDatabase,invK,mScaleFactor);\n keyFramePtr->frameId=frameId;\n keyFramePtr->outId=outIndex;\n \n //set rotation and derolling shutter\n for (int i1=0;i1<3;i1++) {\n for (int i2=0;i2<3;i2++) {\n keyFramePtr->pose.rotation(i1,i2)=globalRotations[globalRotations.size()-1].at(i1,i2);\n }\n }\n for(int i=0;imvLocalMapPoints.size();i++){\n featureList->feature[i]->x=keyFramePtr->framePtr->mvKeys[i].pt.x*mScaleFactor;\n featureList->feature[i]->y=keyFramePtr->framePtr->mvKeys[i].pt.y*mScaleFactor;\n featureList->feature[i]->val=KLT_TRACKED;\n }\n featureList->nFeatures=keyFramePtr->mvLocalMapPoints.size();\n \n /*for(int i=0;imvLocalMapPoints.size();i++){\n \n cv::Mat dst;\n grids.rotateAndNormalizePoint(cv::Point2f(featureList->feature[i]->x,\n featureList->feature[i]->y),\n dst,rotations,cvInvK);\n \n keyFramePtr->mvLocalMapPoints[i].norm(0)=dst.at(0);\n keyFramePtr->mvLocalMapPoints[i].norm(1)=dst.at(1);\n keyFramePtr->mvLocalMapPoints[i].norm(2)=dst.at(2);\n keyFramePtr->mvLocalMapPoints[i].vec=keyFramePtr->mvLocalMapPoints[i].norm\n /keyFramePtr->mvLocalMapPoints[i].norm(2);\n \n \n cv::Vec3b color=colorImage->at(round(featureList->feature[i]->y/2),\n round(featureList->feature[i]->x/2));\n keyFramePtr->mvLocalMapPoints[i].color[0]=color.val[0];\n keyFramePtr->mvLocalMapPoints[i].color[1]=color.val[1];\n keyFramePtr->mvLocalMapPoints[i].color[2]=color.val[2];\n \n keyFramePtr->mvLocalMapPoints[i].tracked.push_back(cv::Point2f(featureList->feature[i]->x,\n featureList->feature[i]->y));\n }*/\n \n \n tbb::parallel_for(tbb::blocked_range(0,keyFramePtr->mvLocalMapPoints.size()),NormalizeKeyFrameInvoker(keyFramePtr,&grids,&featureList,&rotations,&cvInvK,colorImage));\n \n preKeyFrame=frontKeyFrame;\n frontKeyFrame=keyFramePtr;\n frontKeyFrame->prevKeyFramePtr=preKeyFrame;\n }\n printf(\"frame %d end\\n\",frameId);\n pyramidTmpPtr=pyramid1Ptr;\n pyramid1Ptr=pyramid2Ptr;\n pyramid2Ptr=pyramidTmpPtr;\n \n //delete lightFramePre;\n //lightFramePre=lightFrameCur;\n }\n preFrameStamp=frameStamp;\n frameId++;\n return pose;\n }\n \n void System::finish(){\n globalReconstruction.path=path;\n pyramidBuffers.preloadThread.join();\n if(frontKeyFrame->mvLocalFrames.size()>0){\n \n LocalFactorization localFactorization;\n localFactorization.process(frontKeyFrame);\n \n LocalBundleAdjustment localBundleAdjustment;\n localBundleAdjustment.projErrorThres=0.005;\n localBundleAdjustment.viewAngleThres=slamSettings.minViewAngle;\n localBundleAdjustment.bundleAdjust(frontKeyFrame);\n \n localBundleAdjustment.triangulate(frontKeyFrame);\n localBundleAdjustment.bundleAdjust(frontKeyFrame);\n \n char name[200];\n sprintf(name,\"/Users/chaos/Desktop/syndata/data_%d.txt\",frontKeyFrame->frameId);\n frontKeyFrame->saveData2(name);\n sprintf(name,\"/Users/chaos/Desktop/syndata/data_%d.ply\",frontKeyFrame->frameId);\n frontKeyFrame->savePly(name);\n \n if(preKeyFrame->baThread.joinable()){\n preKeyFrame->baThread.join();\n }\n \n keyFrameConnector.mpORBVocabulary=mpVocabulary;\n keyFrameConnector.keyFrameDatabase=mpKeyFrameDatabase;\n keyFrameConnector.connectionThreshold=80;\n keyFrameConnector.connectKeyFrame(preKeyFrame,frontKeyFrame);\n \n mpKeyFrameDatabase->add(frontKeyFrame);\n globalReconstruction.addNewKeyFrame(frontKeyFrame);\n } \n //globalReconstruction.estimateSIM3();\n //globalReconstruction.savePly();\n //globalReconstruction.savePly();\n \n \n \n globalReconstruction.scaleThreshold=20;\n globalReconstruction.estimateScale();\n std::vector index;\n globalReconstruction.estimateRotation(index);\n printf(\"rotation estiamted\\n\");\n globalReconstruction.estimateTranslation(index);\n globalReconstruction.globalRefine();\n globalReconstruction.savePly();\n \n \n //globalReconstruction.frameStart=frameStart;\n globalReconstruction.visualize();\n //globalReconstruction.topview();\n \n //globalReconstruction.savePly();\n \n //globalReconstruction.topview()\n //globalReconstruction.savePly();\n //globalReconstruction.visualize();\n //globalReconstruction.savePly();\n //globalReconstruction.globalPBA();\n //globalReconstruction.estimateSIM3();\n }\n}\n" }, { "path": "GSLAM/System.h", "content": "//\n// System.h\n// GSLAM\n//\n// Created by ctang on 8/30/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"opencv2/core/core.hpp\"\n#include \"ORBVocabulary.h\"\n#include \"KeyFrameDatabase.h\"\n#include \"KeyFrameConnection.h\"\n\n#include \"ORBmatcher.h\"\n#include \"KLT.h\"\n\n//#include \"IMU.hpp\"\n#include \"ImageGrids.hpp\"\n#include \"GlobalReconstruction.h\"\n#include \n#include \n\nusing namespace std;\n\n\n\n\nnamespace GSLAM{\n \n class PyramidBuffer{\n public:\n std::thread preloadThread;\n std::vector* ptrs[3];\n std::vector buffers[3];\n \n void initialize(){\n \n buffers[0]=std::vector();\n buffers[1]=std::vector();\n buffers[2]=std::vector();\n \n ptrs[0]=&buffers[0];\n ptrs[1]=&buffers[1];\n ptrs[2]=&buffers[2];\n }\n \n void next(){\n if(preloadThread.joinable()){\n preloadThread.join();\n }\n std::vector* tmp=ptrs[0];\n ptrs[0]=ptrs[1];\n ptrs[1]=ptrs[2];\n ptrs[2]=tmp;\n }\n };\n \n \n class System{\n \n public:\n \n System(const string &strVocFile, const string &strSettingsFile);\n \n Transform Track(cv::Mat &im, const double ×tamp,const int outIndex);\n \n void finish();\n \n IMU imu;\n \n cv::Mat *colorImage;\n char* path;\n int frameStart;\n int frameEnd;\n \n \n KeyFrame* frontKeyFrame;\n KeyFrame* preKeyFrame;\n SLAMSettings slamSettings;\n KeyFrameConnection keyFrameConnector;\n ORBVocabulary* mpVocabulary;\n KeyFrameDatabase* mpKeyFrameDatabase;\n GlobalReconstruction globalReconstruction;\n \n cv::Mat* preloadImage;\n \n \n \n private:\n \n\n \n //\n int frameId;\n \n //for orb\n\n \n cv::Mat mK;\n cv::Mat mKInv;\n cv::Mat mDistCoef;\n double mScaleFactor;\n \n ORBextractor *mpORBExtractor;\n ORBmatcher *orbMatcher;\n \n //for klt tracking\n PyramidBuffer pyramidBuffers;\n std::vector *pyramid1Ptr,*pyramid2Ptr,*pyramid3Ptr,*pyramidTmpPtr,pyramid1,pyramid2,pyramid3;\n KLT_TrackingContext trackingContext;\n KLT_FeatureList featureList;\n \n //for geometry\n cv::Mat cvInvK;\n Eigen::Matrix3d K,invK;\n std::list activeKeyFrames;\n //KeyFrame* frontKeyFrame;\n //KeyFrame* preKeyFrame;\n std::vector matchesToPre;\n \n //parameters\n CameraSettings cameraSettings;\n ORBExtractorSettings orbExtractorSettings;\n ///SLAMSettings slamSettings;\n \n //temporal parameter for imu\n ImageGrids grids;\n std::vector rotations;\n cv::Mat rotation;\n int frameIndex;\n double frameStamp;\n double preFrameStamp;\n cv::Size frameSize;\n std::vector globalRotations;\n cv::Size sizeByVertex;\n \n //keyframe connection\n //KeyFrameConnection keyFrameConnector;\n \n //globalReconstruction\n \n \n \n //motion lighting compensate\n ORBextractor *mpORBExtractorFrame;\n Frame *lightFramePre,*lightFrameCur;\n };\n}\n" }, { "path": "GSLAM/TrackUtil.cpp", "content": "//\n// TrackUtil.cpp\n// GSLAM\n//\n// Created by ctang on 9/10/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"TrackUtil.h\"\n\n" }, { "path": "GSLAM/TrackUtil.h", "content": "//\n// TrackUtil.h\n// GSLAM\n//\n// Created by ctang on 9/10/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n\n#include \"KLT.h\"\n\n" }, { "path": "GSLAM/Transform.cpp", "content": "//\n// Transform.cpp\n// GSLAM\n//\n// Created by ctang on 9/5/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#include \"Transform.h\"\n#include \"Geometry.h\"\n#include \n\nnamespace GSLAM{\n \n Transform::Transform(){\n scale=1.0;\n rotation.setIdentity();\n translation.setZero();\n }\n \n void Transform::fromCameraToWorld(){\n \n \n \n }\n \n \n void Transform::fromWorldToCamera(){\n \n \n }\n \n void Transform::setEssentialMatrix(){\n setCrossMatrix(E,translation);\n E=rotation*E;\n E/=E(2,2);\n }\n \n Transform Transform::leftMultiply(const Transform& transform)const{\n Transform result;\n result.rotation=transform.rotation*rotation;\n result.translation=rotation.transpose()*transform.translation+translation;\n return result;\n }\n \n Transform Transform::inverse()const{\n Transform result;\n result.rotation=rotation.transpose();\n result.translation=-rotation*translation;\n return result;\n }\n \n void Transform::display() const{\n std::cout<\n#include \n#include \"opencv2/imgproc/imgproc.hpp\"\n\ninline void computeGxGy(const cv::Mat& img,cv::Mat& grad,cv::Mat& filter1,cv::Mat& filter2){\n cv::sepFilter2D(img,grad,CV_32F,filter1,filter2);\n}\n\nvoid computeGradients(const cv::Mat &img,\n cv::Mat &gradx,\n cv::Mat &grady,\n float sigma,\n std::thread* threads){\n \n /* Compute kernels, if necessary */\n if (fabs(sigma - sigma_last) > 0.05){\n computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel);\n }\n //std::cout< 0.05){\n computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel);\n }\n \n cv::Mat kernel(1,gauss_kernel.width,CV_32F,gauss_kernel.data);\n cv::sepFilter2D(img,dst,-1,kernel,kernel);\n}\n\n\n\n/*void sperableFilter2D(cv::Mat& img,cv::Mat& output,cv::Mat& rowFilter,cv::Mat& colFilter){\n \n}\n\nvoid computeGradients2(cv::Mat &img,\n cv::Mat &gradx,\n cv::Mat &grady,\n float sigma){\n \n if (fabs(sigma - sigma_last) > 0.05){\n computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel);\n }\n cv::Mat kernelGauss(1,gauss_kernel.width,CV_32F,gauss_kernel.data);\n cv::Mat kernelGaussDeriv(1,gaussderiv_kernel.width,CV_32F,gaussderiv_kernel.data);\n \n\n \n sperableFilter2D(img,gradx,kernelGaussDeriv,kernelGauss);\n sperableFilter2D(img,grady,kernelGauss,kernelGaussDeriv);\n}\n\n\n\nvoid computeSmoothedImage2(cv::Mat &img,\n cv::Mat &dst,\n float sigma){\n \n if (fabs(sigma - sigma_last) > 0.05){\n computeKernels(sigma, &gauss_kernel, &gaussderiv_kernel);\n }\n cv::Mat kernel(1,gauss_kernel.width,CV_32F,gauss_kernel.data);\n sperableFilter2D(img,dst,kernel,kernel);\n}*/\n\n\n" }, { "path": "GSLAM/convolve.h", "content": "//\n// convolve.hpp\n// STARCK\n//\n// Created by Chaos on 3/29/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n\n#include \"KLT.h\"\n#include \nvoid computeGradients(const cv::Mat &img,cv::Mat &gradx,cv::Mat &grady,float sigma,std::thread* threads=NULL);\nvoid computeSmoothedImage(const cv::Mat &img,cv::Mat &dst,float sigma);\n\n\n\n//void computeGradients2(cv::Mat &img,cv::Mat &gradx,cv::Mat &grady,float sigma);\n//void computeSmoothedImage2(cv::Mat &img,cv::Mat &dst,float sigma);\n\n\n\n\n\n\n" }, { "path": "GSLAM/error.cpp", "content": "/*********************************************************************\n * error.c\n *\n * Error and warning messages, and system commands.\n *********************************************************************/\n\n\n/* Standard includes */\n#include \n#include \n#include \n/*********************************************************************\n * KLTError\n * \n * Prints an error message and dies.\n * \n * INPUTS\n * exactly like printf\n */\n\nvoid KLTError(char *fmt, ...)\n{\n va_list args;\n\n va_start(args, fmt);\n fprintf(stderr, \"KLT Error: \");\n vfprintf(stderr, fmt, args);\n fprintf(stderr, \"\\n\");\n va_end(args);\n exit(1);\n}\n\n\n/*********************************************************************\n * KLTWarning\n * \n * Prints a warning message.\n * \n * INPUTS\n * exactly like printf\n */\n\nvoid KLTWarning(char *fmt, ...)\n{\n va_list args;\n\n va_start(args, fmt);\n fprintf(stderr, \"KLT Warning: \");\n vfprintf(stderr, fmt, args);\n fprintf(stderr, \"\\n\");\n fflush(stderr);\n va_end(args);\n}\n\n" }, { "path": "GSLAM/error.h", "content": "/*********************************************************************\n * error.h\n *********************************************************************/\n\n\n#include \n#include \n\nvoid KLTError(char *fmt, ...);\nvoid KLTWarning(char *fmt, ...);\n\n\n" }, { "path": "GSLAM/experiment_helpers.hpp", "content": "/******************************************************************************\n * Author: Laurent Kneip *\n * Contact: kneip.laurent@gmail.com *\n * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *\n * *\n * Redistribution and use in source and binary forms, with or without *\n * modification, are permitted provided that the following conditions *\n * are met: *\n * * Redistributions of source code must retain the above copyright *\n * notice, this list of conditions and the following disclaimer. *\n * * Redistributions in binary form must reproduce the above copyright *\n * notice, this list of conditions and the following disclaimer in the *\n * documentation and/or other materials provided with the distribution. *\n * * Neither the name of ANU nor the names of its contributors may be *\n * used to endorse or promote products derived from this software without *\n * specific prior written permission. *\n * *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"*\n * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *\n * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *\n * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *\n * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *\n * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *\n * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *\n * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *\n * SUCH DAMAGE. *\n ******************************************************************************/\n\n#ifndef OPENGV_EXPERIMENT_HELPERS_HPP_\n#define OPENGV_EXPERIMENT_HELPERS_HPP_\n\n#include \n#include \n\nnamespace opengv\n{\n\nvoid generateCentralCameraSystem(\n translations_t & camOffsets,\n rotations_t & camRotations );\n\nvoid generateRandomCameraSystem(\n int numberCameras,\n translations_t & camOffsets,\n rotations_t & camRotations );\n\nvoid extractRelativePose(\n const translation_t & position1,\n const translation_t & position2,\n const rotation_t & rotation1,\n const rotation_t & rotation2,\n translation_t & relativePosition,\n rotation_t & relativeRotation,\n bool normalize = true );\n\nvoid printExperimentCharacteristics(\n const translation_t & position,\n const rotation_t & rotation,\n double noise,\n double outlierFraction );\n\nvoid printBearingVectorArraysMatlab(\n const bearingVectors_t & bearingVectors1,\n const bearingVectors_t & bearingVectors2 );\n\nvoid printEssentialMatrix(\n const translation_t & position,\n const rotation_t & rotation);\n\nvoid getPerturbedPose(\n const translation_t & position,\n const rotation_t & rotation,\n translation_t & perturbedPosition,\n rotation_t & perturbedRotation,\n double amplitude );\n\nstd::vector getNindices( int n );\n\n\n\n\nvoid generateRandom2D3DCorrespondences(\n const translation_t & position,\n const rotation_t & rotation,\n const translations_t & camOffsets,\n const rotations_t & camRotations,\n size_t numberPoints,\n double noise,\n double outlierFraction,\n bearingVectors_t & bearingVectors,\n points_t & points,\n std::vector & camCorrespondences,\n Eigen::MatrixXd & gt );\n\nvoid generateMulti2D3DCorrespondences(\n const translation_t & position,\n const rotation_t & rotation,\n const translations_t & camOffsets,\n const rotations_t & camRotations,\n size_t pointsPerCam,\n double noise,\n double outlierFraction,\n std::vector > & multiPoints,\n std::vector > & multiBearingVectors,\n std::vector > & gt );\n\nvoid generateRandom2D2DCorrespondences(\n const translation_t & position1,\n const rotation_t & rotation1,\n const translation_t & position2,\n const rotation_t & rotation2,\n const translations_t & camOffsets,\n const rotations_t & camRotations,\n size_t numberPoints,\n double noise,\n double outlierFraction,\n bearingVectors_t & bearingVectors1,\n bearingVectors_t & bearingVectors2,\n std::vector & camCorrespondences1,\n std::vector & camCorrespondences2,\n Eigen::MatrixXd & gt );\n\nvoid generateRandom3D3DCorrespondences(\n const translation_t & position1,\n const rotation_t & rotation1,\n const translation_t & position2,\n const rotation_t & rotation2,\n size_t numberPoints,\n double noise,\n double outlierFraction,\n bearingVectors_t & points1,\n bearingVectors_t & points2,\n Eigen::MatrixXd & gt );\n\nvoid generateMulti2D2DCorrespondences(\n const translation_t & position1,\n const rotation_t & rotation1,\n const translation_t & position2,\n const rotation_t & rotation2,\n const translations_t & camOffsets,\n const rotations_t & camRotations,\n size_t pointsPerCam,\n double noise,\n double outlierFraction,\n std::vector > & multiBearingVectors1,\n std::vector > & multiBearingVectors2,\n std::vector > & gt );\n\n}\n\n#endif /* OPENGV_EXPERIMENT_HELPERS_HPP_ */\n" }, { "path": "GSLAM/las2.cpp", "content": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n* Neither the name of the University of Tennessee nor the names of its\n contributors may be used to endorse or promote products derived from this\n software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n*/\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \"svdlib.hpp\"\n#include \"svdutil.hpp\"\n\n#define MAXLL 2\n\n#define LMTNW 100000000 /* max. size of working area allowed */\n\nenum storeVals {STORQ = 1, RETRQ, STORP, RETRP};\n\nstatic char *error_msg[] = { /* error messages used by function *\n * check_parameters */\n NULL,\n \"\",\n \"ENDL MUST BE LESS THAN ENDR\",\n \"REQUESTED DIMENSIONS CANNOT EXCEED NUM ITERATIONS\",\n \"ONE OF YOUR DIMENSIONS IS LESS THAN OR EQUAL TO ZERO\",\n \"NUM ITERATIONS (NUMBER OF LANCZOS STEPS) IS INVALID\",\n \"REQUESTED DIMENSIONS (NUMBER OF EIGENPAIRS DESIRED) IS INVALID\",\n \"6*N+4*ITERATIONS+1 + ITERATIONS*ITERATIONS CANNOT EXCEED NW\",\n \"6*N+4*ITERATIONS+1 CANNOT EXCEED NW\", NULL};\n\ndouble **LanStore, *OPBTemp;\ndouble eps, eps1, reps, eps34;\nlong ierr;\n/*\ndouble rnm, anorm, tol;\nFILE *fp_out1, *fp_out2;\n*/\n\nvoid purge(long n, long ll, double *r, double *q, double *ra, \n double *qa, double *wrk, double *eta, double *oldeta, long step, \n double *rnmp, double tol);\nvoid ortbnd(double *alf, double *eta, double *oldeta, double *bet, long step,\n double rnm);\ndouble startv(SMat A, double *wptr[], long step, long n);\nvoid store(long, long, long, double *);\nvoid imtql2(long, long, double *, double *, double *);\nvoid imtqlb(long n, double d[], double e[], double bnd[]);\nvoid write_header(long, long, double, double, long, double, long, long, \n long);\nlong check_parameters(SMat A, long dimensions, long iterations, \n double endl, double endr, long vectors);\nint lanso(SMat A, long iterations, long dimensions, double endl,\n double endr, double *ritz, double *bnd, double *wptr[], \n long *neigp, long n);\nlong ritvec(long n, SMat A, SVDRec R, double kappa, double *ritz, \n double *bnd, double *alf, double *bet, double *w2, \n long steps, long neig);\nlong lanczos_step(SMat A, long first, long last, double *wptr[],\n double *alf, double *eta, double *oldeta,\n double *bet, long *ll, long *enough, double *rnmp, \n double *tolp, long n);\nvoid stpone(SMat A, double *wrkptr[], double *rnmp, double *tolp, long n);\nlong error_bound(long *, double, double, double *, double *, long step, \n double tol);\nvoid machar(long *ibeta, long *it, long *irnd, long *machep, long *negep);\n\n/***********************************************************************\n * *\n * main() *\n * Sparse SVD(A) via Eigensystem of A'A symmetric Matrix \t *\n * (double precision) *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n This sample program uses landr to compute singular triplets of A via\n the equivalent symmetric eigenvalue problem \n\n B x = lambda x, where x' = (u',v'), lambda = sigma**2,\n where sigma is a singular value of A,\n \n B = A'A , and A is m (nrow) by n (ncol) (nrow >> ncol), \n \n so that {u,sqrt(lambda),v} is a singular triplet of A. \n (A' = transpose of A) \n \n User supplied routines: svd_opa, opb, store, timer \n \n svd_opa( x,y) takes an n-vector x and returns A*x in y.\n svd_opb(ncol,x,y) takes an n-vector x and returns B*x in y.\n \n Based on operation flag isw, store(n,isw,j,s) stores/retrieves \n to/from storage a vector of length n in s. \n \n User should edit timer() with an appropriate call to an intrinsic\n timing routine that returns elapsed user time. \n\n\n External parameters \n -------------------\n\n Defined and documented in las2.h\n\n\n Local parameters \n ----------------\n\n (input)\n endl left end of interval containing unwanted eigenvalues of B\n endr right end of interval containing unwanted eigenvalues of B\n kappa relative accuracy of ritz values acceptable as eigenvalues\n of B\n\t vectors is not equal to 1\n r work array\n n\t dimension of the eigenproblem for matrix B (ncol)\n dimensions upper limit of desired number of singular triplets of A\n iterations upper limit of desired number of Lanczos steps\n nnzero number of nonzeros in A\n vectors 1 indicates both singular values and singular vectors are \n\t wanted and they can be found in output file lav2;\n\t 0 indicates only singular values are wanted \n \t\t\n (output)\n ritz\t array of ritz values\n bnd array of error bounds\n d array of singular values\n memory total memory allocated in bytes to solve the B-eigenproblem\n\n\n Functions used\n --------------\n\n BLAS\t\tsvd_daxpy, svd_dscal, svd_ddot\n USER\t\tsvd_opa, svd_opb, timer\n MISC\t\twrite_header, check_parameters\n LAS2\t\tlandr\n\n\n Precision\n ---------\n\n All floating-point calculations are done in double precision;\n variables are declared as long and double.\n\n\n LAS2 development\n ----------------\n\n LAS2 is a C translation of the Fortran-77 LAS2 from the SVDPACK\n library written by Michael W. Berry, University of Tennessee,\n Dept. of Computer Science, 107 Ayres Hall, Knoxville, TN, 37996-1301\n\n 31 Jan 1992: Date written \n\n Theresa H. Do\n University of Tennessee\n Dept. of Computer Science\n 107 Ayres Hall\n Knoxville, TN, 37996-1301\n internet: tdo@cs.utk.edu\n\n ***********************************************************************/\n\n/***********************************************************************\n *\t\t\t\t\t\t\t\t *\n *\t\t check_parameters()\t\t\t *\n *\t\t\t\t\t\t\t\t *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n Function validates input parameters and returns error code (long) \n\n Parameters \n ----------\n (input)\n dimensions upper limit of desired number of eigenpairs of B \n iterations upper limit of desired number of lanczos steps \n n dimension of the eigenproblem for matrix B \n endl left end of interval containing unwanted eigenvalues of B\n endr right end of interval containing unwanted eigenvalues of B\n vectors 1 indicates both eigenvalues and eigenvectors are wanted \n and they can be found in lav2; 0 indicates eigenvalues only\n nnzero number of nonzero elements in input matrix (matrix A) \n \n ***********************************************************************/\n\nlong check_parameters(SMat A, long dimensions, long iterations, \n\t\t double endl, double endr, long vectors) {\n long error_index;\n error_index = 0;\n\n if (endl >/*=*/ endr) error_index = 2;\n else if (dimensions > iterations) error_index = 3;\n else if (A->cols <= 0 || A->rows <= 0) error_index = 4;\n /*else if (n > A->cols || n > A->rows) error_index = 1;*/\n else if (iterations <= 0 || iterations > A->cols || iterations > A->rows)\n error_index = 5;\n else if (dimensions <= 0 || dimensions > iterations) error_index = 6;\n if (error_index) \n svd_error(\"svdLAS2 parameter error: %s\\n\", error_msg[error_index]);\n return(error_index);\n}\n\n/***********************************************************************\n *\t\t\t\t\t\t\t\t *\n *\t\t\t write_header()\t\t\t *\n * Function writes out header of output file containing ritz values *\n *\t\t\t\t\t\t\t\t *\n ***********************************************************************/\n\nvoid write_header(long iterations, long dimensions, double endl, double endr, \n long vectors, double kappa, long nrow, long ncol, \n long vals) {\n printf(\"SOLVING THE [A^TA] EIGENPROBLEM\\n\");\n printf(\"NO. OF ROWS = %6ld\\n\", nrow);\n printf(\"NO. OF COLUMNS = %6ld\\n\", ncol);\n printf(\"NO. OF NON-ZERO VALUES = %6ld\\n\", vals);\n printf(\"MATRIX DENSITY = %6.2f%%\\n\", \n ((float) vals / nrow) * 100 / ncol);\n /* printf(\"ORDER OF MATRIX A = %5ld\\n\", n); */\n printf(\"MAX. NO. OF LANCZOS STEPS = %6ld\\n\", iterations);\n printf(\"MAX. NO. OF EIGENPAIRS = %6ld\\n\", dimensions);\n printf(\"LEFT END OF THE INTERVAL = %9.2E\\n\", endl);\n printf(\"RIGHT END OF THE INTERVAL = %9.2E\\n\", endr);\n printf(\"KAPPA = %9.2E\\n\", kappa);\n /* printf(\"WANT S-VECTORS? [T/F] = %c\\n\", (vectors) ? 'T' : 'F'); */\n printf(\"\\n\");\n return;\n}\n\n\n/***********************************************************************\n * *\n *\t\t\t\tlandr()\t\t\t\t *\n * Lanczos algorithm with selective orthogonalization *\n * Using Simon's Recurrence *\n * (double precision) *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n landr() is the LAS2 driver routine that, upon entry,\n (1) checks for the validity of input parameters of the \n\t B-eigenproblem \n (2) determines several machine constants\n (3) makes a Lanczos run\n (4) calculates B-eigenvectors (singular vectors of A) if requested \n\t by user\n\n\n arguments\n ---------\n\n (input)\n n dimension of the eigenproblem for A'A\n iterations upper limit of desired number of Lanczos steps\n dimensions upper limit of desired number of eigenpairs\n nnzero number of nonzeros in matrix A\n endl left end of interval containing unwanted eigenvalues of B\n endr right end of interval containing unwanted eigenvalues of B\n vectors 1 indicates both eigenvalues and eigenvectors are wanted\n and they can be found in output file lav2; \n\t 0 indicates only eigenvalues are wanted\n kappa relative accuracy of ritz values acceptable as eigenvalues\n\t of B (singular values of A)\n r work array\n\n (output)\n j number of Lanczos steps actually taken \n neig number of ritz values stabilized \n ritz array to hold the ritz values \n bnd array to hold the error bounds\n\n\n External parameters\n -------------------\n\n Defined and documented in las2.h\n\n\n local parameters\n -------------------\n\n ibeta radix for the floating-point representation\n it number of base ibeta digits in the floating-point significand\n irnd floating-point addition rounded or chopped\n machep machine relative precision or round-off error\n negeps largest negative integer\n wptr\t array of pointers each pointing to a work space\n\n\n Functions used\n --------------\n\n MISC svd_dmax, machar, check_parameters\n LAS2 ritvec, lanso\n\n ***********************************************************************/\n\nSVDRec svdLAS2A(SMat A, long dimensions) {\n double end[2] = {-1.0e-30, 1.0e-30};\n double kappa = 1e-30;\n if (!A) {\n svd_error(\"svdLAS2A called with NULL array\\n\");\n return NULL;\n }\n return svdLAS2(A, dimensions, 0, end, kappa);\n}\n\n\nSVDRec svdLAS2(SMat A, long dimensions, long iterations, double end[2], \n double kappa) {\n char transpose = FALSE;\n long ibeta, it, irnd, machep, negep, n, i, steps, nsig, neig, m;\n double *wptr[10], *ritz, *bnd;\n SVDRec R = NULL;\n ierr = 0; // reset the global error flag\n \n svdResetCounters();\n\n m = svd_imin(A->rows, A->cols);\n if (dimensions <= 0 || dimensions > m)\n dimensions = m;\n if (iterations <= 0 || iterations > m)\n iterations = m;\n if (iterations < dimensions) iterations = dimensions;\n\n /* Write output header */\n if (SVDVerbosity > 0)\n write_header(iterations, dimensions, end[0], end[1], TRUE, kappa, A->rows, \n A->cols, A->vals);\n\n /* Check parameters */\n if (check_parameters(A, dimensions, iterations, end[0], end[1], TRUE))\n return NULL;\n\n /* If A is wide, the SVD is computed on its transpose for speed. */\n if (A->cols >= A->rows * 1.2) {\n if (SVDVerbosity > 0) printf(\"TRANSPOSING THE MATRIX FOR SPEED\\n\");\n transpose = TRUE;\n A = svdTransposeS(A);\n }\n\n n = A->cols;\n /* Compute machine precision */ \n machar(&ibeta, &it, &irnd, &machep, &negep);\n eps1 = eps * sqrt((double) n);\n reps = sqrt(eps);\n eps34 = reps * sqrt(reps);\n\n /* Allocate temporary space. */\n if (!(wptr[0] = svd_doubleArray(n, TRUE, \"las2: wptr[0]\"))) goto abort;\n if (!(wptr[1] = svd_doubleArray(n, FALSE, \"las2: wptr[1]\"))) goto abort;\n if (!(wptr[2] = svd_doubleArray(n, FALSE, \"las2: wptr[2]\"))) goto abort;\n if (!(wptr[3] = svd_doubleArray(n, FALSE, \"las2: wptr[3]\"))) goto abort;\n if (!(wptr[4] = svd_doubleArray(n, FALSE, \"las2: wptr[4]\"))) goto abort;\n if (!(wptr[5] = svd_doubleArray(n, FALSE, \"las2: wptr[5]\"))) goto abort;\n if (!(wptr[6] = svd_doubleArray(iterations, FALSE, \"las2: wptr[6]\"))) \n goto abort;\n if (!(wptr[7] = svd_doubleArray(iterations, FALSE, \"las2: wptr[7]\"))) \n goto abort;\n if (!(wptr[8] = svd_doubleArray(iterations, FALSE, \"las2: wptr[8]\"))) \n goto abort;\n if (!(wptr[9] = svd_doubleArray(iterations + 1, FALSE, \"las2: wptr[9]\"))) \n goto abort;\n /* Calloc may be unnecessary: */\n if (!(ritz = svd_doubleArray(iterations + 1, TRUE, \"las2: ritz\"))) \n goto abort; \n /* Calloc may be unnecessary: */\n if (!(bnd = svd_doubleArray(iterations + 1, TRUE, \"las2: bnd\"))) \n goto abort;\n memset(bnd, 127, (iterations + 1) * sizeof(double));\n\n if (!(LanStore = (double **) calloc(iterations + MAXLL, sizeof(double *))))\n goto abort;\n if (!(OPBTemp = svd_doubleArray(A->rows, FALSE, \"las2: OPBTemp\"))) \n goto abort;\n\n /* Actually run the lanczos thing: */\n steps = lanso(A, iterations, dimensions, end[0], end[1], ritz, bnd, wptr, \n &neig, n);\n\n /* Print some stuff. */\n if (SVDVerbosity > 0) {\n printf(\"NUMBER OF LANCZOS STEPS = %6ld\\n\"\n \"RITZ VALUES STABILIZED = %6ld\\n\", steps + 1, neig);\n }\n if (SVDVerbosity > 2) {\n printf(\"\\nCOMPUTED RITZ VALUES (ERROR BNDS)\\n\");\n for (i = 0; i <= steps; i++)\n printf(\"%3ld %22.14E (%11.2E)\\n\", i + 1, ritz[i], bnd[i]);\n }\n\n SAFE_FREE(wptr[0]);\n SAFE_FREE(wptr[1]);\n SAFE_FREE(wptr[2]);\n SAFE_FREE(wptr[3]);\n SAFE_FREE(wptr[4]);\n SAFE_FREE(wptr[7]);\n SAFE_FREE(wptr[8]);\n\n /* Compute eigenvectors */\n kappa = svd_dmax(fabs(kappa), eps34);\n \n R = svdNewSVDRec();\n if (!R) {\n svd_error(\"svdLAS2: allocation of R failed\");\n goto cleanup;\n }\n R->d = /*svd_imin(nsig, dimensions)*/dimensions;\n R->Ut = svdNewDMat(R->d, A->rows);\n R->S = svd_doubleArray(R->d, TRUE, \"las2: R->s\");\n R->Vt = svdNewDMat(R->d, A->cols);\n if (!R->Ut || !R->S || !R->Vt) {\n svd_error(\"svdLAS2: allocation of R failed\");\n goto cleanup;\n }\n\n nsig = ritvec(n, A, R, kappa, ritz, bnd, wptr[6], wptr[9], wptr[5], steps, \n neig);\n \n if (SVDVerbosity > 1) {\n printf(\"\\nSINGULAR VALUES: \");\n svdWriteDenseArray(R->S, R->d, \"-\", FALSE);\n\n if (SVDVerbosity > 2) {\n printf(\"\\nLEFT SINGULAR VECTORS (transpose of U): \");\n svdWriteDenseMatrix(R->Ut, \"-\", SVD_F_DT);\n\n printf(\"\\nRIGHT SINGULAR VECTORS (transpose of V): \");\n svdWriteDenseMatrix(R->Vt, \"-\", SVD_F_DT);\n }\n }\n if (SVDVerbosity > 0) {\n printf(\"SINGULAR VALUES FOUND = %6d\\n\"\n\t \"SIGNIFICANT VALUES = %6ld\\n\", R->d, nsig);\n }\n\n cleanup: \n for (i = 0; i <= 9; i++)\n SAFE_FREE(wptr[i]);\n SAFE_FREE(ritz);\n SAFE_FREE(bnd);\n if (LanStore) {\n for (i = 0; i < iterations + MAXLL; i++)\n SAFE_FREE(LanStore[i]);\n SAFE_FREE(LanStore);\n }\n SAFE_FREE(OPBTemp);\n\n /* This swaps and transposes the singular matrices if A was transposed. */\n if (R && transpose) {\n DMat T;\n svdFreeSMat(A);\n T = R->Ut;\n R->Ut = R->Vt;\n R->Vt = T;\n }\n\n return R;\nabort:\n svd_error(\"svdLAS2: fatal error, aborting\");\n return NULL;\n}\n\n\n/***********************************************************************\n * *\n * ritvec() *\n * \t Function computes the singular vectors of matrix A\t *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n This function is invoked by landr() only if eigenvectors of the A'A\n eigenproblem are desired. When called, ritvec() computes the \n singular vectors of A and writes the result to an unformatted file.\n\n\n Parameters\n ----------\n\n (input)\n nrow number of rows of A\n steps number of Lanczos iterations performed\n fp_out2 pointer to unformatted output file\n n\t dimension of matrix A\n kappa relative accuracy of ritz values acceptable as \n\t\teigenvalues of A'A\n ritz array of ritz values\n bnd array of error bounds\n alf array of diagonal elements of the tridiagonal matrix T\n bet array of off-diagonal elements of T\n w1, w2 work space\n\n (output)\n xv1 array of eigenvectors of A'A (right singular vectors of A)\n ierr\t error code\n 0 for normal return from imtql2()\n\t k if convergence did not occur for k-th eigenvalue in\n\t imtql2()\n nsig number of accepted ritz values based on kappa\n\n (local)\n s\t work array which is initialized to the identity matrix\n\t of order (j + 1) upon calling imtql2(). After the call,\n\t s contains the orthonormal eigenvectors of the symmetric \n\t tridiagonal matrix T\n\n Functions used\n --------------\n\n BLAS\t\tsvd_dscal, svd_dcopy, svd_daxpy\n USER\t\tstore\n \t\timtql2\n\n ***********************************************************************/\n\nvoid rotateArray(double *a, int size, int x) {\n int i, j, n, start;\n double t1, t2;\n if (x == 0) return;\n j = start = 0;\n t1 = a[0];\n for (i = 0; i < size; i++) {\n n = (j >= x) ? j - x : j + size - x;\n t2 = a[n];\n a[n] = t1;\n t1 = t2;\n j = n;\n if (j == start) {\n start = ++j;\n t1 = a[j];\n }\n }\n}\n\nlong ritvec(long n, SMat A, SVDRec R, double kappa, double *ritz, double *bnd, \n double *alf, double *bet, double *w2, long steps, long neig) {\n\n long js, jsq, i, k, /*size,*/ id2, tmp, nsig, x;\n double *s, *xv2, tmp0, tmp1, xnorm, *w1 = R->Vt->value[0];\n \n js = steps + 1;\n jsq = js * js;\n /*size = sizeof(double) * n;*/\n \n s = svd_doubleArray(jsq, TRUE, \"ritvec: s\");\n xv2 = svd_doubleArray(n, FALSE, \"ritvec: xv2\");\n \n /* initialize s to an identity matrix */\n for (i = 0; i < jsq; i+= (js+1)) s[i] = 1.0;\n svd_dcopy(js, alf, 1, w1, -1);\n svd_dcopy(steps, &bet[1], 1, &w2[1], -1);\n \n /* on return from imtql2(), w1 contains eigenvalues in ascending \n * order and s contains the corresponding eigenvectors */\n imtql2(js, js, w1, w2, s);\n \n /*fwrite((char *)&n, sizeof(n), 1, fp_out2);\n fwrite((char *)&js, sizeof(js), 1, fp_out2);\n fwrite((char *)&kappa, sizeof(kappa), 1, fp_out2);*/\n /*id = 0;*/\n nsig = 0;\n\n if (ierr) {\n R->d = 0;\n } else {\n x = 0;\n id2 = jsq - js;\n for (k = 0; k < js; k++) {\n tmp = id2;\n if (bnd[k] <= kappa * fabs(ritz[k]) && k > js-neig-1) {\n\tif (--x < 0) x = R->d - 1;\n\tw1 = R->Vt->value[x];\n\tfor (i = 0; i < n; i++) w1[i] = 0.0;\n\tfor (i = 0; i < js; i++) {\n\t store(n, RETRQ, i, w2);\n\t svd_daxpy(n, s[tmp], w2, 1, w1, 1);\n\t tmp -= js;\n\t}\n\t/*fwrite((char *)w1, size, 1, fp_out2);*/\n \n\t/* store the w1 vector row-wise in array xv1; \n\t * size of xv1 is (steps+1) * (nrow+ncol) elements \n\t * and each vector, even though only ncol long,\n\t * will have (nrow+ncol) elements in xv1. \n\t * It is as if xv1 is a 2-d array (steps+1) by \n\t * (nrow+ncol) and each vector occupies a row */\n\n\t/* j is the index in the R arrays, which are sorted by high to low \n\t singular values. */\n \n\t/*for (i = 0; i < n; i++) R->Vt->value[x]xv1[id++] = w1[i];*/\n\t/*id += nrow;*/\n\tnsig++;\n }\n id2++;\n }\n SAFE_FREE(s);\n\n /* Rotate the singular vectors and values. */\n /* x is now the location of the highest singular value. */\n rotateArray(R->Vt->value[0], R->Vt->rows * R->Vt->cols, \n\t\tx * R->Vt->cols);\n R->d = svd_imin(R->d, nsig);\n for (x = 0; x < R->d; x++) {\n /* multiply by matrix B first */\n svd_opb(A, R->Vt->value[x], xv2, OPBTemp);\n tmp0 = svd_ddot(n, R->Vt->value[x], 1, xv2, 1);\n svd_daxpy(n, -tmp0, R->Vt->value[x], 1, xv2, 1);\n tmp0 = sqrt(tmp0);\n xnorm = sqrt(svd_ddot(n, xv2, 1, xv2, 1));\n \n /* multiply by matrix A to get (scaled) left s-vector */\n svd_opa(A, R->Vt->value[x], R->Ut->value[x]);\n tmp1 = 1.0 / tmp0;\n svd_dscal(A->rows, tmp1, R->Ut->value[x], 1);\n xnorm *= tmp1;\n bnd[i] = xnorm;\n R->S[x] = tmp0;\n }\n }\n\n SAFE_FREE(s);\n SAFE_FREE(xv2);\n return nsig;\n}\n\n/***********************************************************************\n * *\n * lanso() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Function determines when the restart of the Lanczos algorithm should \n occur and when it should terminate.\n\n Arguments \n ---------\n\n (input)\n n dimension of the eigenproblem for matrix B\n iterations upper limit of desired number of lanczos steps \n dimensions upper limit of desired number of eigenpairs \n endl left end of interval containing unwanted eigenvalues\n endr right end of interval containing unwanted eigenvalues\n ritz array to hold the ritz values \n bnd array to hold the error bounds \n wptr array of pointers that point to work space: \n \t wptr[0]-wptr[5] six vectors of length n\t\t\n \t wptr[6] array to hold diagonal of the tridiagonal matrix T\n \t wptr[9] array to hold off-diagonal of T\t\n \t wptr[7] orthogonality estimate of Lanczos vectors at \n\t\t step j\n \t wptr[8] orthogonality estimate of Lanczos vectors at \n\t\t step j-1\n\n (output)\n j number of Lanczos steps actually taken\n neig number of ritz values stabilized\n ritz array to hold the ritz values\n bnd array to hold the error bounds\n ierr (globally declared) error flag\n\t ierr = 8192 if stpone() fails to find a starting vector\n\t ierr = k if convergence did not occur for k-th eigenvalue\n\t\t in imtqlb()\n\t ierr = 0 otherwise\n\n\n Functions used\n --------------\n\n LAS\t\tstpone, error_bound, lanczos_step\n MISC\t\tsvd_dsort2\n UTILITY\tsvd_imin, svd_imax\n\n ***********************************************************************/\n\nint lanso(SMat A, long iterations, long dimensions, double endl,\n double endr, double *ritz, double *bnd, double *wptr[], \n long *neigp, long n) {\n double *alf, *eta, *oldeta, *bet, *wrk, rnm, tol;\n long ll, first, last, ENOUGH, id2, id3, i, l, neig, j = 0, intro = 0;\n \n alf = wptr[6];\n eta = wptr[7];\n oldeta = wptr[8];\n bet = wptr[9];\n wrk = wptr[5];\n \n /* take the first step */\n stpone(A, wptr, &rnm, &tol, n);\n if (!rnm || ierr) return 0;\n eta[0] = eps1;\n oldeta[0] = eps1;\n ll = 0;\n first = 1;\n last = svd_imin(dimensions + svd_imax(8, dimensions), iterations);\n ENOUGH = FALSE;\n /*id1 = 0;*/\n int nIterations=0;\n while (/*id1 < dimensions && */!ENOUGH) {\n if (rnm <= tol) rnm = 0.0;\n \n /* the actual lanczos loop */\n j = lanczos_step(A, first, last, wptr, alf, eta, oldeta, bet, &ll,\n &ENOUGH, &rnm, &tol, n);\n nIterations++;\n if (ENOUGH) j = j - 1;\n else j = last - 1;\n first = j + 1;\n bet[j+1] = rnm;\n \n /* analyze T */\n l = 0;\n for (id2 = 0; id2 < j; id2++) {\n if (l > j) break;\n for (i = l; i <= j; i++) if (!bet[i+1]) break;\n if (i > j) i = j;\n \n /* now i is at the end of an unreduced submatrix */\n svd_dcopy(i-l+1, &alf[l], 1, &ritz[l], -1);\n svd_dcopy(i-l, &bet[l+1], 1, &wrk[l+1], -1);\n \n imtqlb(i-l+1, &ritz[l], &wrk[l], &bnd[l]);\n \n if (ierr) {\n svd_error(\"svdLAS2: imtqlb failed to converge (ierr = %ld)\\n\", ierr);\n svd_error(\" l = %ld i = %ld\\n\", l, i);\n for (id3 = l; id3 <= i; id3++) \n svd_error(\" %ld %lg %lg %lg\\n\", \n id3, ritz[id3], wrk[id3], bnd[id3]);\n }\n for (id3 = l; id3 <= i; id3++) \n bnd[id3] = rnm * fabs(bnd[id3]);\n l = i + 1;\n }\n \n /* sort eigenvalues into increasing order */\n svd_dsort2((j+1) / 2, j + 1, ritz, bnd);\n\n /* for (i = 0; i < iterations; i++)\n printf(\"%f \", ritz[i]);\n printf(\"\\n\"); */\n \n /* massage error bounds for very close ritz values */\n neig = error_bound(&ENOUGH, endl, endr, ritz, bnd, j, tol);\n *neigp = neig;\n \n /* should we stop? */\n if (neig < dimensions) {\n if (!neig) {\n last = first + 9;\n intro = first;\n } else last = first + svd_imax(3, 1 + ((j - intro) * (dimensions-neig)) /\n neig);\n last = svd_imin(last, iterations);\n } else ENOUGH = TRUE;\n ENOUGH = ENOUGH || first >= iterations;\n /* id1++; */\n /* printf(\"id1=%d dimen=%d first=%d\\n\", id1, dimensions, first); */\n \n }\n printf(\"niterations %d\\n\",nIterations);\n store(n, STORQ, j, wptr[1]);\n return j;\n}\n\n\n/***********************************************************************\n * *\n *\t\t\tlanczos_step() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Function embodies a single Lanczos step\n\n Arguments \n ---------\n\n (input)\n n dimension of the eigenproblem for matrix B\n first start of index through loop\t\t\t\t \n last end of index through loop\t\t\t\t \n wptr\t array of pointers pointing to work space\t\t \n alf\t array to hold diagonal of the tridiagonal matrix T\n eta orthogonality estimate of Lanczos vectors at step j \n oldeta orthogonality estimate of Lanczos vectors at step j-1\n bet array to hold off-diagonal of T \n ll number of intitial Lanczos vectors in local orthog. \n (has value of 0, 1 or 2)\t\t\t\n enough stop flag\t\t\t\n\n Functions used\n --------------\n\n BLAS\t\tsvd_ddot, svd_dscal, svd_daxpy, svd_datx, svd_dcopy\n USER\t\tstore\n LAS\t\tpurge, ortbnd, startv\n UTILITY\tsvd_imin, svd_imax\n\n ***********************************************************************/\n\nlong lanczos_step(SMat A, long first, long last, double *wptr[],\n\t\t double *alf, double *eta, double *oldeta,\n\t\t double *bet, long *ll, long *enough, double *rnmp, \n double *tolp, long n) {\n double t, *mid, rnm = *rnmp, tol = *tolp, anorm;\n long i, j;\n\n for (j=first; j 4.0 * fabs(alf[j])))\n\t *ll = j; \n for (i=0; i < svd_imin(*ll, j-1); i++) {\n\t store(n, RETRP, i, wptr[5]);\n\t t = svd_ddot(n, wptr[5], 1, wptr[0], 1);\n\t store(n, RETRQ, i, wptr[5]);\n svd_daxpy(n, -t, wptr[5], 1, wptr[0], 1);\n\t eta[i] = eps1;\n\t oldeta[i] = eps1;\n }\n\n /* extended local reorthogonalization */\n t = svd_ddot(n, wptr[0], 1, wptr[4], 1);\n svd_daxpy(n, -t, wptr[2], 1, wptr[0], 1);\n if (bet[j] > 0.0) bet[j] = bet[j] + t;\n t = svd_ddot(n, wptr[0], 1, wptr[3], 1);\n svd_daxpy(n, -t, wptr[1], 1, wptr[0], 1);\n alf[j] = alf[j] + t;\n svd_dcopy(n, wptr[0], 1, wptr[4], 1);\n rnm = sqrt(svd_ddot(n, wptr[0], 1, wptr[4], 1));\n anorm = bet[j] + fabs(alf[j]) + rnm;\n tol = reps * anorm;\n\n /* update the orthogonality bounds */\n ortbnd(alf, eta, oldeta, bet, j, rnm);\n\n /* restore the orthogonality state when needed */\n purge(n, *ll, wptr[0], wptr[1], wptr[4], wptr[3], wptr[5], eta, oldeta,\n j, &rnm, tol);\n if (rnm <= tol) rnm = 0.0;\n }\n *rnmp = rnm;\n *tolp = tol;\n return j;\n}\n\n/***********************************************************************\n * *\n * ortbnd() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Funtion updates the eta recurrence\n\n Arguments \n ---------\n\n (input)\n alf array to hold diagonal of the tridiagonal matrix T \n eta orthogonality estimate of Lanczos vectors at step j \n oldeta orthogonality estimate of Lanczos vectors at step j-1 \n bet array to hold off-diagonal of T \n n dimension of the eigenproblem for matrix B\t\t \n j dimension of T\t\t\t\t\t \n rnm\t norm of the next residual vector\t\t\t \n eps1\t roundoff estimate for dot product of two unit vectors\n\n (output)\n eta orthogonality estimate of Lanczos vectors at step j+1 \n oldeta orthogonality estimate of Lanczos vectors at step j \n\n\n Functions used\n --------------\n\n BLAS\t\tsvd_dswap\n\n ***********************************************************************/\n\nvoid ortbnd(double *alf, double *eta, double *oldeta, double *bet, long step,\n double rnm) {\n long i;\n if (step < 1) return;\n if (rnm) {\n if (step > 1) {\n\t oldeta[0] = (bet[1] * eta[1] + (alf[0]-alf[step]) * eta[0] -\n\t\t bet[step] * oldeta[0]) / rnm + eps1;\n }\n for (i=1; i<=step-2; i++) \n\t oldeta[i] = (bet[i+1] * eta[i+1] + (alf[i]-alf[step]) * eta[i] +\n\t\t bet[i] * eta[i-1] - bet[step] * oldeta[i])/rnm + eps1;\n }\n oldeta[step-1] = eps1;\n svd_dswap(step, oldeta, 1, eta, 1); \n eta[step] = eps1;\n return;\n}\n\n/***********************************************************************\n * *\n *\t\t\t\tpurge() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Function examines the state of orthogonality between the new Lanczos\n vector and the previous ones to decide whether re-orthogonalization \n should be performed\n\n\n Arguments \n ---------\n\n (input)\n n dimension of the eigenproblem for matrix B\t\t \n ll number of intitial Lanczos vectors in local orthog. \n r residual vector to become next Lanczos vector \n q current Lanczos vector\t\t\t \n ra previous Lanczos vector\n qa previous Lanczos vector\n wrk temporary vector to hold the previous Lanczos vector\n eta state of orthogonality between r and prev. Lanczos vectors \n oldeta state of orthogonality between q and prev. Lanczos vectors\n j current Lanczos step\t\t\t\t \n\n (output)\n r\t residual vector orthogonalized against previous Lanczos \n\t vectors\n q current Lanczos vector orthogonalized against previous ones\n\n\n Functions used\n --------------\n\n BLAS\t\tsvd_daxpy, svd_dcopy, svd_idamax, svd_ddot\n USER\t\tstore\n\n ***********************************************************************/\n\nvoid purge(long n, long ll, double *r, double *q, double *ra, \n\t double *qa, double *wrk, double *eta, double *oldeta, long step, \n double *rnmp, double tol) {\n double t, tq, tr, reps1, rnm = *rnmp;\n long k, iteration, flag, i;\n \n if (step < ll+2) return; \n \n k = svd_idamax(step - (ll+1), &eta[ll], 1) + ll;\n if (fabs(eta[k]) > reps) {\n reps1 = eps1 / reps;\n iteration = 0;\n flag = TRUE;\n while (iteration < 2 && flag) {\n if (rnm > tol) {\n \n /* bring in a lanczos vector t and orthogonalize both \n * r and q against it */\n tq = 0.0;\n tr = 0.0;\n for (i = ll; i < step; i++) {\n store(n, RETRQ, i, wrk);\n t = -svd_ddot(n, qa, 1, wrk, 1);\n tq += fabs(t);\n svd_daxpy(n, t, wrk, 1, q, 1);\n t = -svd_ddot(n, ra, 1, wrk, 1);\n tr += fabs(t);\n svd_daxpy(n, t, wrk, 1, r, 1);\n }\n svd_dcopy(n, q, 1, qa, 1);\n t = -svd_ddot(n, r, 1, qa, 1);\n tr += fabs(t);\n svd_daxpy(n, t, q, 1, r, 1);\n svd_dcopy(n, r, 1, ra, 1);\n rnm = sqrt(svd_ddot(n, ra, 1, r, 1));\n if (tq <= reps1 && tr <= reps1 * rnm) flag = FALSE;\n }\n iteration++;\n }\n for (i = ll; i <= step; i++) { \n eta[i] = eps1;\n oldeta[i] = eps1;\n }\n }\n *rnmp = rnm;\n return;\n}\n\n\n/***********************************************************************\n * *\n * stpone() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Function performs the first step of the Lanczos algorithm. It also\n does a step of extended local re-orthogonalization.\n\n Arguments \n ---------\n\n (input)\n n dimension of the eigenproblem for matrix B\n\n (output)\n ierr error flag\n wptr array of pointers that point to work space that contains\n\t wptr[0] r[j]\n\t wptr[1] q[j]\n\t wptr[2] q[j-1]\n\t wptr[3] p\n\t wptr[4] p[j-1]\n\t wptr[6] diagonal elements of matrix T \n\n\n Functions used\n --------------\n\n BLAS\t\tsvd_daxpy, svd_datx, svd_dcopy, svd_ddot, svd_dscal\n USER\t\tstore, opb\n LAS\t\tstartv\n\n ***********************************************************************/\n\nvoid stpone(SMat A, double *wrkptr[], double *rnmp, double *tolp, long n) {\n double t, *alf, rnm, anorm;\n alf = wrkptr[6];\n\n /* get initial vector; default is random */\n rnm = startv(A, wrkptr, 0, n);\n if (rnm == 0.0 || ierr != 0) return;\n\n /* normalize starting vector */\n t = 1.0 / rnm;\n svd_datx(n, t, wrkptr[0], 1, wrkptr[1], 1);\n svd_dscal(n, t, wrkptr[3], 1);\n\n /* take the first step */\n svd_opb(A, wrkptr[3], wrkptr[0], OPBTemp);\n alf[0] = svd_ddot(n, wrkptr[0], 1, wrkptr[3], 1);\n svd_daxpy(n, -alf[0], wrkptr[1], 1, wrkptr[0], 1);\n t = svd_ddot(n, wrkptr[0], 1, wrkptr[3], 1);\n svd_daxpy(n, -t, wrkptr[1], 1, wrkptr[0], 1);\n alf[0] += t;\n svd_dcopy(n, wrkptr[0], 1, wrkptr[4], 1);\n rnm = sqrt(svd_ddot(n, wrkptr[0], 1, wrkptr[4], 1));\n anorm = rnm + fabs(alf[0]);\n *rnmp = rnm;\n *tolp = reps * anorm;\n\n return;\n}\n\n/***********************************************************************\n * *\n * startv() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Function delivers a starting vector in r and returns |r|; it returns \n zero if the range is spanned, and ierr is non-zero if no starting \n vector within range of operator can be found.\n\n Parameters \n ---------\n\n (input)\n n dimension of the eigenproblem matrix B\n wptr array of pointers that point to work space\n j starting index for a Lanczos run\n eps machine epsilon (relative precision)\n\n (output)\n wptr array of pointers that point to work space that contains\n\t r[j], q[j], q[j-1], p[j], p[j-1]\n ierr error flag (nonzero if no starting vector can be found)\n\n Functions used\n --------------\n\n BLAS\t\tsvd_ddot, svd_dcopy, svd_daxpy\n USER\t\tsvd_opb, store\n MISC\t\trandom\n\n ***********************************************************************/\n\ndouble startv(SMat A, double *wptr[], long step, long n) {\n double rnm2, *r, t;\n long irand;\n long id, i;\n\n /* get initial vector; default is random */\n rnm2 = svd_ddot(n, wptr[0], 1, wptr[0], 1);\n irand = 918273 + step;\n r = wptr[0];\n for (id = 0; id < 3; id++) {\n if (id > 0 || step > 0 || rnm2 == 0) \n\t for (i = 0; i < n; i++) r[i] = svd_random2(&irand);\n svd_dcopy(n, wptr[0], 1, wptr[3], 1);\n\n /* apply operator to put r in range (essential if m singular) */\n svd_opb(A, wptr[3], wptr[0], OPBTemp);\n svd_dcopy(n, wptr[0], 1, wptr[3], 1);\n rnm2 = svd_ddot(n, wptr[0], 1, wptr[3], 1);\n if (rnm2 > 0.0) break;\n }\n\n /* fatal error */\n if (rnm2 <= 0.0) {\n ierr = 8192;\n return(-1);\n }\n if (step > 0) {\n for (i = 0; i < step; i++) {\n store(n, RETRQ, i, wptr[5]);\n\t t = -svd_ddot(n, wptr[3], 1, wptr[5], 1);\n\t svd_daxpy(n, t, wptr[5], 1, wptr[0], 1);\n }\n\n /* make sure q[step] is orthogonal to q[step-1] */\n t = svd_ddot(n, wptr[4], 1, wptr[0], 1);\n svd_daxpy(n, -t, wptr[2], 1, wptr[0], 1);\n svd_dcopy(n, wptr[0], 1, wptr[3], 1);\n t = svd_ddot(n, wptr[3], 1, wptr[0], 1);\n if (t <= eps * rnm2) t = 0.0;\n rnm2 = t;\n }\n return(sqrt(rnm2));\n}\n\n/***********************************************************************\n * *\n *\t\t\terror_bound() *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n Function massages error bounds for very close ritz values by placing \n a gap between them. The error bounds are then refined to reflect \n this.\n\n\n Arguments \n ---------\n\n (input)\n endl left end of interval containing unwanted eigenvalues\n endr right end of interval containing unwanted eigenvalues\n ritz array to store the ritz values\n bnd array to store the error bounds\n enough stop flag\n\n\n Functions used\n --------------\n\n BLAS\t\tsvd_idamax\n UTILITY\tsvd_dmin\n\n ***********************************************************************/\n\nlong error_bound(long *enough, double endl, double endr, \n double *ritz, double *bnd, long step, double tol) {\n long mid, i, neig;\n double gapl, gap;\n \n /* massage error bounds for very close ritz values */\n mid = svd_idamax(step + 1, bnd, 1);\n\n for (i=((step+1) + (step-1)) / 2; i >= mid + 1; i -= 1)\n if (fabs(ritz[i-1] - ritz[i]) < eps34 * fabs(ritz[i])) \n if (bnd[i] > tol && bnd[i-1] > tol) {\n bnd[i-1] = sqrt(bnd[i] * bnd[i] + bnd[i-1] * bnd[i-1]);\n bnd[i] = 0.0;\n }\n \n \n for (i=((step+1) - (step-1)) / 2; i <= mid - 1; i +=1 ) \n if (fabs(ritz[i+1] - ritz[i]) < eps34 * fabs(ritz[i])) \n if (bnd[i] > tol && bnd[i+1] > tol) {\n bnd[i+1] = sqrt(bnd[i] * bnd[i] + bnd[i+1] * bnd[i+1]);\n bnd[i] = 0.0;\n }\n \n /* refine the error bounds */\n neig = 0;\n gapl = ritz[step] - ritz[0];\n for (i = 0; i <= step; i++) {\n gap = gapl;\n if (i < step) gapl = ritz[i+1] - ritz[i];\n gap = svd_dmin(gap, gapl);\n if (gap > bnd[i]) bnd[i] = bnd[i] * (bnd[i] / gap);\n if (bnd[i] <= 16.0 * eps * fabs(ritz[i])) {\n neig++;\n if (!*enough) *enough = endl < ritz[i] && ritz[i] < endr;\n }\n } \n return neig;\n}\n\n/***********************************************************************\n * *\n *\t\t\t\timtqlb()\t\t\t *\n * *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n imtqlb() is a translation of a Fortran version of the Algol\n procedure IMTQL1, Num. Math. 12, 377-383(1968) by Martin and \n Wilkinson, as modified in Num. Math. 15, 450(1970) by Dubrulle. \n Handbook for Auto. Comp., vol.II-Linear Algebra, 241-248(1971). \n See also B. T. Smith et al, Eispack Guide, Lecture Notes in \n Computer Science, Springer-Verlag, (1976).\n\n The function finds the eigenvalues of a symmetric tridiagonal\n matrix by the implicit QL method.\n\n\n Arguments \n ---------\n\n (input)\n n order of the symmetric tridiagonal matrix \n d contains the diagonal elements of the input matrix \n e contains the subdiagonal elements of the input matrix in its\n last n-1 positions. e[0] is arbitrary\t \n\n (output)\n d contains the eigenvalues in ascending order. if an error\n exit is made, the eigenvalues are correct and ordered for\n indices 0,1,...ierr, but may not be the smallest eigenvalues.\n e has been destroyed.\t\t\t\t\t \n ierr set to zero for normal return, j if the j-th eigenvalue has\n not been determined after 30 iterations.\t\t \n\n Functions used\n --------------\n\n UTILITY\tsvd_fsign\n MISC\t\tsvd_pythag\n\n ***********************************************************************/\n\nvoid imtqlb(long n, double d[], double e[], double bnd[])\n\n{\n long last, l, m, i, iteration;\n\n /* various flags */\n long exchange, convergence, underflow;\t\n\n double b, test, g, r, s, c, p, f;\n\n if (n == 1) return;\n ierr = 0;\n bnd[0] = 1.0;\n last = n - 1;\n for (i = 1; i < n; i++) {\n bnd[i] = 0.0;\n e[i-1] = e[i];\n }\n e[last] = 0.0;\n for (l = 0; l < n; l++) {\n iteration = 0;\n while (iteration <= 30) {\n\t for (m = l; m < n; m++) {\n\t convergence = FALSE;\n\t if (m == last) break;\n\t else {\n\t test = fabs(d[m]) + fabs(d[m+1]);\n\t if (test + fabs(e[m]) == test) convergence = TRUE;\n\t }\n\t if (convergence) break;\n\t }\n\t p = d[l]; \n\t f = bnd[l]; \n\t if (m != l) {\n\t if (iteration == 30) {\n\t ierr = l;\n\t return;\n\t }\n\t iteration += 1;\n\t /*........ form shift ........*/\n\t g = (d[l+1] - p) / (2.0 * e[l]);\n\t r = svd_pythag(g, 1.0);\n\t g = d[m] - p + e[l] / (g + svd_fsign(r, g));\n\t s = 1.0;\n\t c = 1.0;\n\t p = 0.0;\n\t underflow = FALSE;\n\t i = m - 1;\n\t while (underflow == FALSE && i >= l) {\n\t f = s * e[i];\n\t b = c * e[i];\n\t r = svd_pythag(f, g);\n\t e[i+1] = r;\n\t if (r == 0.0) underflow = TRUE;\n\t else {\n\t\t s = f / r;\n\t\t c = g / r;\n\t\t g = d[i+1] - p;\n\t\t r = (d[i] - g) * s + 2.0 * c * b;\n\t\t p = s * r;\n\t\t d[i+1] = g + p;\n\t\t g = c * r - b;\n\t\t f = bnd[i+1];\n\t\t bnd[i+1] = s * bnd[i] + c * f;\n\t\t bnd[i] = c * bnd[i] - s * f;\n\t\t i--;\n\t }\n\t } /* end while (underflow != FALSE && i >= l) */\n\t /*........ recover from underflow .........*/\n\t if (underflow) {\n\t d[i+1] -= p;\n\t e[m] = 0.0;\n\t }\n\t else {\n\t d[l] -= p;\n\t e[l] = g;\n\t e[m] = 0.0;\n\t }\n\t } \t\t \t\t /* end if (m != l) */\n\t else {\n\n /* order the eigenvalues */\n\t exchange = TRUE;\n\t if (l != 0) {\n\t i = l;\n\t while (i >= 1 && exchange == TRUE) {\n\t if (p < d[i-1]) {\n\t\t d[i] = d[i-1];\n\t\t bnd[i] = bnd[i-1];\n\t i--;\n\t }\n\t else exchange = FALSE;\n\t }\n\t }\n\t if (exchange) i = 0;\n\t d[i] = p;\n\t bnd[i] = f; \n\t iteration = 31;\n\t }\n }\t\t\t /* end while (iteration <= 30) */\n }\t\t\t\t /* end for (l=0; l= l) {\n\t f = s * e[i];\n\t b = c * e[i];\n\t r = svd_pythag(f, g);\n\t e[i+1] = r;\n\t if (r == 0.0) underflow = TRUE;\n\t else {\n\t\t s = f / r;\n\t\t c = g / r;\n\t\t g = d[i+1] - p;\n\t\t r = (d[i] - g) * s + 2.0 * c * b;\n\t\t p = s * r;\n\t\t d[i+1] = g + p;\n\t\t g = c * r - b;\n\n\t\t /* form vector */\n\t\t for (k = 0; k < nnm; k += n) {\n\t\t index = k + i;\n\t\t f = z[index+1];\n\t\t z[index+1] = s * z[index] + c * f;\n\t\t z[index] = c * z[index] - s * f;\n\t\t } \n\t\t i--;\n\t }\n\t } /* end while (underflow != FALSE && i >= l) */\n\t /*........ recover from underflow .........*/\n\t if (underflow) {\n\t d[i+1] -= p;\n\t e[m] = 0.0;\n\t }\n\t else {\n\t d[l] -= p;\n\t e[l] = g;\n\t e[m] = 0.0;\n\t }\n\t }\n\t else break;\n }\t\t/*...... end while (iteration <= 30) .........*/\n }\t\t/*...... end for (l=0; l= MAXLL) {\n svd_error(\"svdLAS2: store (STORP) called with j >= MAXLL\");\n break;\n }\n if (!LanStore[j]) {\n if (!(LanStore[j] = svd_doubleArray(n, FALSE, \"LanStore[j]\")))\n svd_fatalError(\"svdLAS2: failed to allocate LanStore[%d]\", j);\n }\n svd_dcopy(n, s, 1, LanStore[j], 1);\n break;\n case RETRP:\t\n if (j >= MAXLL) {\n svd_error(\"svdLAS2: store (RETRP) called with j >= MAXLL\");\n break;\n }\n if (!LanStore[j])\n svd_fatalError(\"svdLAS2: store (RETRP) called on index %d (not allocated)\", \n j);\n svd_dcopy(n, LanStore[j], 1, s, 1);\n break;\n }\n return;\n}\n" }, { "path": "GSLAM/main.cpp", "content": "//\n// main.cpp\n// STAR\n//\n// Created by Chaos on 4/25/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n#include \"opencv2/imgproc/imgproc.hpp\"\n#import \n#import \n#import \n#include \"System.h\"\n#include \"GlobalReconstruction.h\"\n#include \n#include \n\nint main(int argc, char **argv){\n \n printf(\"version no global ptr\\n\");\n system(\"exec rm -r /Users/chaos/Desktop/debug/*\");\n \n argv[1]=\"/Users/chaos/Downloads/sequences/robot\";\n argv[2]=\"/Users/chaos/Downloads/sequences/ORBvoc.txt\";\n std::cout<<\"process \"< yuv(3);\n cv::split(uv,&yuv[1]);\n cv::resize(image,yuv[0],cv::Size(width/2,height/2));\n cv::Mat rgb;\n cv::merge((const cv::Mat*)(&yuv[0]),3,rgb);\n cv::cvtColor(rgb,rgb,CV_YCrCb2RGB);\n \n double timestamp;\n timestamps>>timestamp;\n \n if(frameCount==slamSystem.frameStart){\n \n slamSystem.colorImage=&rgb;\n image.copyTo(curImage);\n \n }else if(frameCount>slamSystem.frameStart){\n image.copyTo(nextImage);\n slamSystem.preloadImage=&nextImage;\n std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();\n GSLAM::Transform pose=slamSystem.Track(curImage,timestamp,frameCount);\n std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();\n double ttrack= std::chrono::duration_cast >(t2 - t1).count();\n record<slamSystem.frameEnd){\n break;\n }\n }\n\n record.close();\n slamSystem.finish();\n cout<<\"processed \"<\n#include \"opencv2/core/core.hpp\"\n//#include \"opencv2/video/tracking.hpp\"\n\n//void computePyramid(KLT_TrackingContext tc,\n// const cv::Mat &img,\n// std::vector &pyramid){\n// \n// if (pyramid.size()!=3*tc->nPyramidLevels) {\n// pyramid.resize(3*tc->nPyramidLevels);\n// }\n// \n// int ncols = img.cols, nrows = img.rows;\n// float pyramid_sigma = tc->subsampling * tc->pyramid_sigma_fact; /* empirically determined */\n// \n// cv::Mat tmpimg;\n// img.convertTo(tmpimg,CV_32F);\n// \n// computeSmoothedImage(tmpimg,pyramid[0],_KLTComputeSmoothSigma(tc));\n// computeGradients(pyramid[0],pyramid[1],pyramid[2],tc->grad_sigma);\n// for (int i = 1 ; i < tc->nPyramidLevels ; i++) {\n// computeSmoothedImage(pyramid[3*i-3],tmpimg,pyramid_sigma);\n// ncols /= tc->subsampling; nrows /= tc->subsampling;\n// cv::resize(tmpimg,pyramid[3*i],cv::Size(ncols,nrows));\n// computeGradients(pyramid[3*i],pyramid[3*i+1],pyramid[3*i+2],tc->grad_sigma);\n// }\n//}\n\n#include \n#include \"KLTUtil.h\"\ninline void computeGxGy(const cv::Mat& img,cv::Mat& grad,cv::Mat& filter1,cv::Mat& filter2){\n cv::sepFilter2D(img,grad,CV_32F,filter1,filter2);\n}\n\nvoid computePyramid(KLT_TrackingContext tc,\n const cv::Mat &img,\n std::vector &pyramid){\n \n if (pyramid.size()!=3*tc->nPyramidLevels) {\n pyramid.resize(3*tc->nPyramidLevels);\n }\n \n \n \n int ncols = img.cols, nrows = img.rows;\n float pyramid_sigma = tc->subsampling * tc->pyramid_sigma_fact; /* empirically determined */\n \n cv::Mat tmpimg;\n img.convertTo(tmpimg,CV_32F);\n \n computeSmoothedImage(tmpimg,pyramid[0],_KLTComputeSmoothSigma(tc));\n for (int i = 1 ; i < tc->nPyramidLevels ; i++) {\n computeSmoothedImage(pyramid[3*i-3],tmpimg,pyramid_sigma);\n ncols /= tc->subsampling; nrows /= tc->subsampling;\n cv::resize(tmpimg,pyramid[3*i],cv::Size(ncols,nrows));\n }\n \n \n if (fabs(tc->grad_sigma - sigma_last) > 0.05){\n computeKernels(tc->grad_sigma,&gauss_kernel, &gaussderiv_kernel);\n }\n cv::Mat kernelGauss(1,gauss_kernel.width,CV_32F,gauss_kernel.data);\n cv::Mat kernelGaussDeriv(1,gaussderiv_kernel.width,CV_32F,gaussderiv_kernel.data);\n \n std::vector threads(2*tc->nPyramidLevels);\n for (int i = 0 ; i < tc->nPyramidLevels ; i++) {\n threads[2*i]=std::thread(computeGxGy,std::ref(pyramid[3*i]),std::ref(pyramid[3*i+1]),std::ref(kernelGaussDeriv),std::ref(kernelGauss));\n threads[2*i+1]=std::thread(computeGxGy,std::ref(pyramid[3*i]),std::ref(pyramid[3*i+2]),std::ref(kernelGauss),std::ref(kernelGaussDeriv));\n }\n \n for (int i=0;i &pyramid);\n" }, { "path": "GSLAM/random_generators.hpp", "content": "/******************************************************************************\n * Author: Laurent Kneip *\n * Contact: kneip.laurent@gmail.com *\n * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *\n * *\n * Redistribution and use in source and binary forms, with or without *\n * modification, are permitted provided that the following conditions *\n * are met: *\n * * Redistributions of source code must retain the above copyright *\n * notice, this list of conditions and the following disclaimer. *\n * * Redistributions in binary form must reproduce the above copyright *\n * notice, this list of conditions and the following disclaimer in the *\n * documentation and/or other materials provided with the distribution. *\n * * Neither the name of ANU nor the names of its contributors may be *\n * used to endorse or promote products derived from this software without *\n * specific prior written permission. *\n * *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"*\n * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *\n * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *\n * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *\n * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *\n * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *\n * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *\n * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *\n * SUCH DAMAGE. *\n ******************************************************************************/\n\n#ifndef OPENGV_RANDOM_GENERATORS_HPP_\n#define OPENGV_RANDOM_GENERATORS_HPP_\n\n#include \n#include \n#include \n\nnamespace opengv\n{\n\nvoid initializeRandomSeed();\nEigen::Vector3d generateRandomPoint( double maximumDepth, double minimumDepth );\nEigen::Vector3d generateRandomPointPlane();\nEigen::Vector3d addNoise( double noiseLevel, Eigen::Vector3d cleanPoint );\nEigen::Vector3d generateRandomTranslation( double maximumParallax );\nEigen::Vector3d generateRandomDirectionTranslation( double parallax );\nEigen::Matrix3d generateRandomRotation( double maxAngle );\nEigen::Matrix3d generateRandomRotation();\n\n}\n\n#endif /* OPENGV_RANDOM_GENERATORS_HPP_ */\n" }, { "path": "GSLAM/selectGoodFeatures.hpp", "content": "//\n// selectGoodFeatures.hpp\n// STARCK\n//\n// Created by Chaos on 3/30/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n\n/*********************************************************************\n * selectGoodFeatures.c\n *\n *********************************************************************/\n\n/* Standard includes */\n#include \n#include /* malloc(), qsort() */\n#include /* fflush() */\n#include /* memset() */\n#include \"opencv2/core/core.hpp\"\n#include \"KLT.h\"\n\n#pragma once\n\n#define KLT_NOT_FOUND -1\n//#define USE_DILATE\n\ncv::Size gridSize(20,20);\ncv::Size sizeByGrid;\nint minFeatureInGrid;\n\n\n\ntypedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode;\n\n\nstatic void _fillFeaturemap(int x, int y,\n uchar *featuremap,\n int mindist,\n int ncols,\n int nrows){\n int ix, iy;\n \n for (iy = y - mindist ; iy <= y + mindist ; iy++){\n for (ix = x - mindist ; ix <= x + mindist ; ix++)\n if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows)\n featuremap[iy*ncols+ix] = 1;\n }\n}\n\n/*********************************************************************\n * _enforceMinimumDistance\n *\n * Removes features that are within close proximity to better features.\n *\n * INPUTS\n * featurelist: A list of features. The nFeatures property\n * is used.\n *\n * OUTPUTS\n * featurelist: Is overwritten. Nearby \"redundant\" features are removed.\n * Writes -1's into the remaining elements.\n *\n * RETURNS\n * The number of remaining features.\n \n \n */\n\nstatic int _comparePoints(const void *a, const void *b)\n{\n int v1 = *(((int *) a) + 2);\n int v2 = *(((int *) b) + 2);\n \n if (v1 > v2) return(-1);\n else if (v1 < v2) return(1);\n else return(0);\n}\n\nstatic void _sortPointList(\n int *pointlist,\n int npoints){\n qsort(pointlist, npoints, 3*sizeof(int), _comparePoints);\n}\n\n\n\nstatic void _enforceMinimumDistance(uchar *featuremap,\n int *pointlist, /* featurepoints */\n int npoints, /* number of featurepoints */\n KLT_FeatureList featurelist, /* features */\n int ncols, int nrows, /* size of images */\n int mindist, /* min. dist b/w features */\n int min_eigenvalue, /* min. eigenvalue */\n bool overwriteAllFeatures)\n{\n int indx; /* Index into features */\n int x, y, val; /* Location and trackability of pixel under consideration */\n //int *featuremap; /* Boolean array recording proximity of features */\n int *ptr;\n \n /* Cannot add features with an eigenvalue less than one */\n if (min_eigenvalue < 1)\n min_eigenvalue = 1;\n \n /* Allocate memory for feature map and clear it */\n //featuremap = (int *) malloc(ncols * nrows * sizeof(int));\n //memset(featuremap, 0, ncols*nrows*sizeof(int));\n \n /* Necessary because code below works with (mindist-1) */\n mindist--;\n \n /* If we are keeping all old good features, then add them to the featuremap */\n \n /*if (!overwriteAllFeatures)\n for (indx = 0 ; indx < featurelist->nFeatures ; indx++)\n if (featurelist->feature[indx]->val >= 0) {\n x = (int) featurelist->feature[indx]->x;\n y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y,INT_MAX,featuremap, mindist, ncols, nrows);\n }\n */\n \n /* For each feature point, in descending order of importance, do ... */\n ptr = pointlist;\n indx = 0;\n while (1) {\n \n /* If we can't add all the points, then fill in the rest\n of the featurelist with -1's */\n if (ptr >= pointlist + 3*npoints) {\n while (indx < featurelist->nFeatures) {\n if (overwriteAllFeatures ||\n featurelist->feature[indx]->val < 0) {\n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = KLT_NOT_FOUND;\n \n /*\n featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_img_gradx = NULL;\n featurelist->feature[indx]->aff_img_grady = NULL;\n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;*/\n \n }\n indx++;\n }\n break;\n }\n \n x = *ptr++;\n y = *ptr++;\n val = *ptr++;\n \n /* Ensure that feature is in-bounds */\n assert(x >= 0);\n assert(x < ncols);\n assert(y >= 0);\n assert(y < nrows);\n \n while (!overwriteAllFeatures &&\n indx < featurelist->nFeatures &&\n featurelist->feature[indx]->val >= 0)\n indx++;\n \n if (indx >= featurelist->nFeatures) break;\n \n /* If no neighbor has been selected, and if the minimum\n eigenvalue is large enough, then add feature to the current list */\n if (!(featuremap[y*ncols+x]) && val >= min_eigenvalue) {\n \n featurelist->feature[indx]->x = (float) x;\n featurelist->feature[indx]->y = (float) y;\n featurelist->feature[indx]->val = (int) val;\n \n /*\n featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_img_gradx = NULL;\n featurelist->feature[indx]->aff_img_grady = NULL;\n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;*/\n indx++;\n \n /* Fill in surrounding region of feature map, but\n make sure that pixels are in-bounds */\n _fillFeaturemap(x, y,featuremap, mindist, ncols, nrows);\n }\n }\n \n /* Free feature map */\n //free(featuremap);\n}\n\n\n\n/*********************************************************************\n * _sortPointList\n */\n\n\n\n/*********************************************************************\n * _minEigenvalue\n *\n * Given the three distinct elements of the symmetric 2x2 matrix\n * [gxx gxy]\n * [gxy gyy],\n * Returns the minimum eigenvalue of the matrix.\n */\n\nstatic float _minEigenvalue(float gxx, float gxy, float gyy)\n{\n return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f);\n}\n\nstruct greaterThanPtr :\npublic std::binary_function\n{\n bool operator () (const float * a, const float * b) const\n { return *a > *b; }\n};\n\n/*********************************************************************/\n\nvoid _KLTSelectGoodFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList featurelist,\n selectionMode mode)\n{\n int npoints = 0;\n bool overwriteAllFeatures = (mode == SELECTING_ALL) ? true : false;\n bool floatimages_created = false;\n \n int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1;\n \n\n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"Tracking context's window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"Tracking context's window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));\n \n\n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n if (!overwriteAllFeatures){\n for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y,featuremap,tc->mindist, ncols, nrows);\n }\n }\n }\n \n#ifdef USE_DILATE\n cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F);\n#endif\n \n int nSkippedPixels=tc->nSkippedPixels;\n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n /* For most of the pixels in the image, do ... */\n ptr = pointlist;\n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n#ifdef USE_DILATE\n float* eigPtr=(float*)eig.ptr(y);\n#endif\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]) {\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float val= _minEigenvalue(gxx, gxy, gyy);\n \n \n if (val > limit) {\n val = (float) limit;\n }\n \n if (val>tc->min_eigenvalue){\n#ifdef USE_DILATE\n eigPtr[x] = val;\n#else\n *ptr++ = x;\n *ptr++ = y;\n *ptr++ = (int) val;\n npoints++;\n#endif\n }\n }\n }\n }\n\n#ifdef USE_DILATE\n \n cv::Mat tmp;\n cv::dilate(eig,tmp,cv::Mat());\n cv::Size imgsize=tmp.size();\n \n std::vector tmpCorners;\n for( int y = 1; y < imgsize.height - 1; y++ )\n {\n const float* eig_data = (const float*)eig.ptr(y);\n const float* tmp_data = (const float*)tmp.ptr(y);\n\n for( int x = 1; x < imgsize.width - 1; x++ )\n {\n float val = eig_data[x];\n if( val != 0&& val == tmp_data[x])\n tmpCorners.push_back(eig_data + x);\n }\n }\n \n std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr());\n \n npoints=tmpCorners.size();\n for (int i=0;imindist,\n tc->min_eigenvalue,\n overwriteAllFeatures);\n \n /* Free memory */\n free(featuremap);\n free(pointlist);\n}\n\n\n\n\nvoid KLTSelectGoodFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL);\n}\n\n\nvoid KLTReplaceLostFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n if (nLostFeatures > 0)\n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME);\n \n}\n\ntypedef enum {SELECTING_ALL2, REPLACING_SOME2} selectionMode2;\n\n\nstatic void _fillFeaturemap2(int x, int y,\n uchar *featuremap,\n int mindist,\n int ncols,\n int nrows){\n int ix, iy;\n \n for (iy = y - mindist ; iy <= y + mindist ; iy++){\n for (ix = x - mindist ; ix <= x + mindist ; ix++)\n if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows)\n featuremap[iy*ncols+ix] = 1;\n }\n}\n\n/*********************************************************************\n * _enforceMinimumDistance\n *\n * Removes features that are within close proximity to better features.\n *\n * INPUTS\n * featurelist: A list of features. The nFeatures property\n * is used.\n *\n * OUTPUTS\n * featurelist: Is overwritten. Nearby \"redundant\" features are removed.\n * Writes -1's into the remaining elements.\n *\n * RETURNS\n * The number of remaining features.\n \n \n */\n\nstatic int _comparePoints2(const void *a, const void *b)\n{\n int v1 = *(((int *) a) + 2);\n int v2 = *(((int *) b) + 2);\n \n if (v1 > v2) return(-1);\n else if (v1 < v2) return(1);\n else return(0);\n}\n\nstatic void _sortPointList2(\n int *pointlist,\n int npoints){\n qsort(pointlist, npoints, 3*sizeof(int), _comparePoints2);\n}\n\nstatic float _minEigenvalue2(float gxx, float gxy, float gyy)\n{\n return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f);\n}\nvoid _enforceMinimumDistance2(uchar* featuremap,\n int *pointlist, /* featurepoints */\n int npoints, /* number of featurepoints */\n KLT_FeatureList featurelist, /* features */\n int ncols, int nrows, /* size of images */\n int mindist, /* min. dist b/w features */\n int min_eigenvalue, /* min. eigenvalue */\n bool overwriteAllFeatures)\n{\n int indx; /* Index into features */\n int x, y, val; /* Location and trackability of pixel under consideration */\n /* Boolean array recording proximity of features */\n int *ptr;\n \n /* Cannot add features with an eigenvalue less than one */\n if (min_eigenvalue < 1)\n min_eigenvalue = 1;\n \n //mindist--;\n /* Allocate memory for feature map and clear it */\n \n /*\n featuremap = (uchar *) malloc(ncols * nrows * sizeof(uchar));\n memset(featuremap, 0, ncols*nrows);\n \n \n \n if (!overwriteAllFeatures)\n for (indx = 0 ; indx < featurelist->nFeatures ; indx++)\n if (featurelist->feature[indx]->val >= 0) {\n x = (int) featurelist->feature[indx]->x;\n y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows);\n }\n */\n \n /* For each feature point, in descending order of importance, do ... */\n ptr = pointlist;\n indx = 0;\n while (1) {\n \n /* If we can't add all the points, then fill in the rest\n of the featurelist with -1's */\n if (ptr >= pointlist + 3*npoints) {\n while (indx < featurelist->nFeatures) {\n if (overwriteAllFeatures ||\n featurelist->feature[indx]->val < 0) {\n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = KLT_NOT_FOUND;\n \n /*featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_img_gradx = NULL;\n featurelist->feature[indx]->aff_img_grady = NULL;*/\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n }\n indx++;\n }\n break;\n }\n \n x = *ptr++;\n y = *ptr++;\n val = *ptr++;\n \n /* Ensure that feature is in-bounds */\n assert(x >= 0);\n assert(x < ncols);\n assert(y >= 0);\n assert(y < nrows);\n \n while (!overwriteAllFeatures &&\n indx < featurelist->nFeatures &&\n featurelist->feature[indx]->val >= 0)\n indx++;\n \n if (indx >= featurelist->nFeatures) break;\n \n /* If no neighbor has been selected, and if the minimum\n eigenvalue is large enough, then add feature to the current list */\n if (!featuremap[y*ncols+x] && val >= min_eigenvalue) {\n featurelist->feature[indx]->x = (KLT_locType) x;\n featurelist->feature[indx]->y = (KLT_locType) y;\n featurelist->feature[indx]->val = (int) val;\n \n /*featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_gradx = NULL;\n featurelist->feature[indx]->aff_grady = NULL;*/\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n indx++;\n \n /* Fill in surrounding region of feature map, but\n make sure that pixels are in-bounds */\n _fillFeaturemap2(x, y, featuremap, mindist, ncols, nrows);\n }\n }\n \n /* Free feature map */\n //free(featuremap);\n}\n\nvoid _KLTSelectGoodFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList featurelist,\n selectionMode2 mode)\n{\n int npoints = 0;\n bool overwriteAllFeatures = (mode == SELECTING_ALL2) ? true : false;\n bool floatimages_created = false;\n \n int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1;\n \n \n \n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n \n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n \n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n \n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));\n \n \n \n \n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n if (!overwriteAllFeatures){\n for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap2(x, y,featuremap,tc->mindist, ncols, nrows);\n }\n }\n }\n#ifdef USE_DILATE\n cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F);\n#endif\n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n /* For most of the pixels in the image, do ... */\n ptr = pointlist;\n int nSkippedPixels=tc->nSkippedPixels;\n \n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n#ifdef USE_DILATE\n float* eigPtr=(float*)eig.ptr(y);\n#endif\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]==1) {\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float _val= _minEigenvalue2(gxx, gxy, gyy);\n int val;\n \n if (_val > limit) {\n val = limit;\n }else{\n val=(int)(_val+0.5);\n }\n \n if (val>tc->min_eigenvalue){\n#ifdef USE_DILATE\n eigPtr[x] = val;\n#else\n *ptr++ = x;\n *ptr++ = y;\n *ptr++ = val;\n npoints++;\n#endif\n }\n }\n }\n }\n //printf(\"%d noints\\n\",npoints);\n#ifdef USE_DILATE\n \n cv::Mat tmp;\n cv::dilate(eig,tmp,cv::Mat());\n cv::Size imgsize=tmp.size();\n \n std::vector tmpCorners;\n for( int y = 1; y < imgsize.height - 1; y++ )\n {\n const float* eig_data = (const float*)eig.ptr(y);\n const float* tmp_data = (const float*)tmp.ptr(y);\n \n for( int x = 1; x < imgsize.width - 1; x++ )\n {\n float val = eig_data[x];\n if( val != 0&& val == tmp_data[x])\n tmpCorners.push_back(eig_data + x);\n }\n }\n \n std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr());\n \n npoints=tmpCorners.size();\n for (int i=0;imindist,\n tc->min_eigenvalue,\n overwriteAllFeatures);\n \n \n /* Free memory */\n free(featuremap);\n free(pointlist);\n}\n\nvoid _KLTSelectGoodFeatures2(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList featurelist,\n selectionMode mode)\n{\n int npoints = 0;\n bool overwriteAllFeatures = (mode == SELECTING_ALL) ? true : false;\n bool floatimages_created = false;\n \n int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1;\n \n \n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"Tracking context's window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"Tracking context's window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));\n \n \n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n if (!overwriteAllFeatures){\n for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y,featuremap,tc->mindist, ncols, nrows);\n }\n }\n }\n \n#ifdef USE_DILATE\n cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F);\n#endif\n \n int nSkippedPixels=tc->nSkippedPixels;\n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n /* For most of the pixels in the image, do ... */\n ptr = pointlist;\n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n#ifdef USE_DILATE\n float* eigPtr=(float*)eig.ptr(y);\n#endif\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]) {\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float val= _minEigenvalue(gxx, gxy, gyy);\n \n \n if (val > limit) {\n val = (float) limit;\n }\n \n if (val>tc->min_eigenvalue){\n#ifdef USE_DILATE\n eigPtr[x] = val;\n#else\n *ptr++ = x;\n *ptr++ = y;\n *ptr++ = (int) val;\n npoints++;\n#endif\n }\n }\n }\n }\n \n#ifdef USE_DILATE\n \n cv::Mat tmp;\n cv::dilate(eig,tmp,cv::Mat());\n cv::Size imgsize=tmp.size();\n \n std::vector tmpCorners;\n for( int y = 1; y < imgsize.height - 1; y++ )\n {\n const float* eig_data = (const float*)eig.ptr(y);\n const float* tmp_data = (const float*)tmp.ptr(y);\n \n for( int x = 1; x < imgsize.width - 1; x++ )\n {\n float val = eig_data[x];\n if( val != 0&& val == tmp_data[x])\n tmpCorners.push_back(eig_data + x);\n }\n }\n \n std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr());\n \n npoints=tmpCorners.size();\n for (int i=0;imindist,\n tc->min_eigenvalue,\n overwriteAllFeatures);\n \n /* Free memory */\n free(featuremap);\n free(pointlist);\n}\n\n\n\nvoid KLTSelectGoodFeatures2(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n _KLTSelectGoodFeatures2(tc,sumDxx,sumDyy,sumDxy,fl,SELECTING_ALL);\n}\n\n\nvoid KLTReplaceLostFeatures2(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n if (nLostFeatures > 0)\n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME2);\n \n}\n\n\n\nstruct greaterThanPtrInt:\npublic std::binary_function\n{\n bool operator () (const int * a, const int * b) const\n { return *a > *b; }\n};\n\nstruct smallerDistance:\npublic std::binary_function\n{\n bool operator () (const int a, const int b) const{\n \n float distanceY=a/sizeByGrid.width-float(sizeByGrid.height)/2.0;\n float distanceX=a%sizeByGrid.width-float(sizeByGrid.width)/2.0;\n \n float distanceA=distanceX*distanceX+distanceY*distanceY;\n \n distanceY=b/sizeByGrid.width-float(sizeByGrid.height)/2.0;\n distanceX=b%sizeByGrid.width-float(sizeByGrid.width)/2.0;\n \n float distanceB=distanceX*distanceX+distanceY*distanceY;\n \n return distanceA gridSequence;\n\nvoid initializeGridSequence(){\n gridSequence.resize(sizeByGrid.area());\n for(int i=0;iwindow_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"Tracking context's window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"Tracking context's window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n \n \n \n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n \n //grid and eigmap\n std::vector > grids;\n grids.resize(sizeByGrid.width*sizeByGrid.height);\n for(int i=0;inFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n \n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x,y,featuremap,tc->mindist, ncols, nrows);\n \n \n //register to grid\n int* eigPtr=(int*)eig.ptr(y);\n eigPtr[x]=(int)INT_MAX;\n \n int yByGrid=y/gridSize.height;\n int xByGrid=x/gridSize.width;\n grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]);\n }\n }\n }\n \n int nSkippedPixels=tc->nSkippedPixels;\n \n\n \n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n int* eigPtr=(int*)eig.ptr(y);\n\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]) {\n //assert(0);\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float val= _minEigenvalue(gxx, gxy, gyy);\n \n \n if (val > limit) {\n assert(0);\n val = (float) limit;\n }\n \n if (val>tc->min_eigenvalue){\n eigPtr[x]=(int)val;\n int yByGrid=y/gridSize.height;\n int xByGrid=x/gridSize.width;\n grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]);\n }\n }\n }\n }\n \n \n for(int i=0;imaxFeatureInGrid&&val!=INT_MAX){\n break;\n }*/\n\n \n if(val!=INT_MAX){\n \n int ofs = (int)((const uchar*)grids[i][j] - eig.ptr());\n int y = (int)(ofs / eig.step);\n int x = (int)((ofs-y*eig.step)/sizeof(int));\n \n \n while (!overwriteAllFeatures &&indx < featurelist->nFeatures&& featurelist->feature[indx]->val >= 0){\n indx++;\n }\n \n if (indx >= featurelist->nFeatures){\n if(featureAdded>minFeatureInGrid){\n break;\n }else{\n \n featurelist->nFeatures++;\n \n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = -1;\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n \n }\n }\n \n \n if (!featuremap[y*ncols+x]) {\n \n featurelist->feature[indx]->x = (KLT_locType) x;\n featurelist->feature[indx]->y = (KLT_locType) y;\n featurelist->feature[indx]->val = (int) val;\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n indx++;\n \n _fillFeaturemap(x, y, featuremap,tc->mindist, ncols, nrows);\n featureAdded++;\n }\n }\n }\n }\n \n \n while (indx < featurelist->nFeatures) {\n \n if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) {\n \n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = KLT_NOT_FOUND;\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n }\n indx++;\n }\n \n free(featuremap);\n \n}\n\nvoid KLTSelectGoodFeaturesUniform(\n KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n \n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n \n sizeByGrid.width=ncols/gridSize.width;\n sizeByGrid.height=nrows/gridSize.height;\n \n minFeatureInGrid=int((fl->nFeatures/float(sizeByGrid.area()))+0.5);\n initializeGridSequence();\n \n \n _KLTSelectGoodFeaturesUniform(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL);\n}\n\n\nvoid KLTReplaceLostFeaturesUniform(\n KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n if (nLostFeatures > 0)\n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME);\n \n}\n\n\n\n\n\n\n\n\n\n" }, { "path": "GSLAM/selectGoodFeatures2.hpp", "content": "//\n// selectGoodFeatures.hpp\n// STARCK\n//\n// Created by Chaos on 3/30/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n\n/*********************************************************************\n * selectGoodFeatures.c\n *\n *********************************************************************/\n\n/* Standard includes */\n#include \n#include /* malloc(), qsort() */\n#include /* fflush() */\n#include /* memset() */\n#include \"KLT.h\"\n\n#pragma once\n\n#define KLT_NOT_FOUND -1\n\n\ncv::Size gridSize(20,20);\ncv::Size sizeByGrid;\nint minFeatureInGrid;\n\n\n\ntypedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode;\n\n\nstatic void _fillFeaturemap(int x, int y,\n uchar *featuremap,\n int mindist,\n int ncols,\n int nrows){\n int ix, iy;\n \n for (iy = y - mindist ; iy <= y + mindist ; iy++){\n for (ix = x - mindist ; ix <= x + mindist ; ix++)\n if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows)\n featuremap[iy*ncols+ix] = 1;\n }\n}\n\n/*********************************************************************\n * _enforceMinimumDistance\n *\n * Removes features that are within close proximity to better features.\n *\n * INPUTS\n * featurelist: A list of features. The nFeatures property\n * is used.\n *\n * OUTPUTS\n * featurelist: Is overwritten. Nearby \"redundant\" features are removed.\n * Writes -1's into the remaining elements.\n *\n * RETURNS\n * The number of remaining features.\n \n \n */\n\nstatic int _comparePoints(const void *a, const void *b)\n{\n int v1 = *(((int *) a) + 2);\n int v2 = *(((int *) b) + 2);\n \n if (v1 > v2) return(-1);\n else if (v1 < v2) return(1);\n else return(0);\n}\n\nstatic void _sortPointList(\n int *pointlist,\n int npoints){\n qsort(pointlist, npoints, 3*sizeof(int), _comparePoints);\n}\n\n\n\nstatic void _enforceMinimumDistance(uchar *featuremap,\n int *pointlist, /* featurepoints */\n int npoints, /* number of featurepoints */\n KLT_FeatureList featurelist, /* features */\n int ncols, int nrows, /* size of images */\n int mindist, /* min. dist b/w features */\n int min_eigenvalue, /* min. eigenvalue */\n bool overwriteAllFeatures)\n{\n int indx; /* Index into features */\n int x, y, val; /* Location and trackability of pixel under consideration */\n //int *featuremap; /* Boolean array recording proximity of features */\n int *ptr;\n \n /* Cannot add features with an eigenvalue less than one */\n if (min_eigenvalue < 1)\n min_eigenvalue = 1;\n \n /* Allocate memory for feature map and clear it */\n //featuremap = (int *) malloc(ncols * nrows * sizeof(int));\n //memset(featuremap, 0, ncols*nrows*sizeof(int));\n \n /* Necessary because code below works with (mindist-1) */\n mindist--;\n \n /* If we are keeping all old good features, then add them to the featuremap */\n \n /*if (!overwriteAllFeatures)\n for (indx = 0 ; indx < featurelist->nFeatures ; indx++)\n if (featurelist->feature[indx]->val >= 0) {\n x = (int) featurelist->feature[indx]->x;\n y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y,INT_MAX,featuremap, mindist, ncols, nrows);\n }\n */\n \n /* For each feature point, in descending order of importance, do ... */\n ptr = pointlist;\n indx = 0;\n while (1) {\n \n /* If we can't add all the points, then fill in the rest\n of the featurelist with -1's */\n if (ptr >= pointlist + 3*npoints) {\n while (indx < featurelist->nFeatures) {\n if (overwriteAllFeatures ||\n featurelist->feature[indx]->val < 0) {\n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = KLT_NOT_FOUND;\n \n /*\n featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_img_gradx = NULL;\n featurelist->feature[indx]->aff_img_grady = NULL;\n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;*/\n \n }\n indx++;\n }\n break;\n }\n \n x = *ptr++;\n y = *ptr++;\n val = *ptr++;\n \n /* Ensure that feature is in-bounds */\n assert(x >= 0);\n assert(x < ncols);\n assert(y >= 0);\n assert(y < nrows);\n \n while (!overwriteAllFeatures &&\n indx < featurelist->nFeatures &&\n featurelist->feature[indx]->val >= 0)\n indx++;\n \n if (indx >= featurelist->nFeatures) break;\n \n /* If no neighbor has been selected, and if the minimum\n eigenvalue is large enough, then add feature to the current list */\n if (!(featuremap[y*ncols+x]) && val >= min_eigenvalue) {\n \n featurelist->feature[indx]->x = (float) x;\n featurelist->feature[indx]->y = (float) y;\n featurelist->feature[indx]->val = (int) val;\n \n /*\n featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_img_gradx = NULL;\n featurelist->feature[indx]->aff_img_grady = NULL;\n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;*/\n indx++;\n \n /* Fill in surrounding region of feature map, but\n make sure that pixels are in-bounds */\n _fillFeaturemap(x, y,featuremap, mindist, ncols, nrows);\n }\n }\n \n /* Free feature map */\n //free(featuremap);\n}\n\n\n\n/*********************************************************************\n * _sortPointList\n */\n\n\n\n/*********************************************************************\n * _minEigenvalue\n *\n * Given the three distinct elements of the symmetric 2x2 matrix\n * [gxx gxy]\n * [gxy gyy],\n * Returns the minimum eigenvalue of the matrix.\n */\n\nstatic float _minEigenvalue(float gxx, float gxy, float gyy)\n{\n return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f);\n}\n\nstruct greaterThanPtr :\npublic std::binary_function\n{\n bool operator () (const float * a, const float * b) const\n { return *a > *b; }\n};\n\n/*********************************************************************/\n\nvoid _KLTSelectGoodFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList featurelist,\n selectionMode mode)\n{\n int npoints = 0;\n bool overwriteAllFeatures = (mode == SELECTING_ALL) ? true : false;\n bool floatimages_created = false;\n \n int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1;\n \n\n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"Tracking context's window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"Tracking context's window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));\n \n\n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n if (!overwriteAllFeatures){\n for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y,featuremap,tc->mindist, ncols, nrows);\n }\n }\n }\n \n#ifdef USE_DILATE\n cv::Mat eig=cv::Mat::zeros(nrows,ncols,CV_32F);\n#endif\n \n int nSkippedPixels=tc->nSkippedPixels;\n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n /* For most of the pixels in the image, do ... */\n ptr = pointlist;\n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n#ifdef USE_DILATE\n float* eigPtr=(float*)eig.ptr(y);\n#endif\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]) {\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float val= _minEigenvalue(gxx, gxy, gyy);\n \n \n if (val > limit) {\n val = (float) limit;\n }\n \n if (val>tc->min_eigenvalue){\n#ifdef USE_DILATE\n eigPtr[x] = val;\n#else\n *ptr++ = x;\n *ptr++ = y;\n *ptr++ = (int) val;\n npoints++;\n#endif\n }\n }\n }\n }\n\n#ifdef USE_DILATE\n \n cv::Mat tmp;\n cv::dilate(eig,tmp,cv::Mat());\n cv::Size imgsize=tmp.size();\n \n std::vector tmpCorners;\n for( int y = 1; y < imgsize.height - 1; y++ )\n {\n const float* eig_data = (const float*)eig.ptr(y);\n const float* tmp_data = (const float*)tmp.ptr(y);\n\n for( int x = 1; x < imgsize.width - 1; x++ )\n {\n float val = eig_data[x];\n if( val != 0&& val == tmp_data[x])\n tmpCorners.push_back(eig_data + x);\n }\n }\n \n std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr());\n \n npoints=tmpCorners.size();\n for (int i=0;imindist,\n tc->min_eigenvalue,\n overwriteAllFeatures);\n \n /* Free memory */\n free(featuremap);\n free(pointlist);\n}\n\n\n\n\nvoid KLTSelectGoodFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL);\n}\n\n\nvoid KLTReplaceLostFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n if (nLostFeatures > 0)\n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME);\n \n}\n\ntypedef enum {SELECTING_ALL2, REPLACING_SOME2} selectionMode2;\n\n\nstatic void _fillFeaturemap2(int x, int y,\n uchar *featuremap,\n int mindist,\n int ncols,\n int nrows){\n int ix, iy;\n \n for (iy = y - mindist ; iy <= y + mindist ; iy++){\n for (ix = x - mindist ; ix <= x + mindist ; ix++)\n if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows)\n featuremap[iy*ncols+ix] = 1;\n }\n}\n\n/*********************************************************************\n * _enforceMinimumDistance\n *\n * Removes features that are within close proximity to better features.\n *\n * INPUTS\n * featurelist: A list of features. The nFeatures property\n * is used.\n *\n * OUTPUTS\n * featurelist: Is overwritten. Nearby \"redundant\" features are removed.\n * Writes -1's into the remaining elements.\n *\n * RETURNS\n * The number of remaining features.\n \n \n */\n\nstatic int _comparePoints2(const void *a, const void *b)\n{\n int v1 = *(((int *) a) + 2);\n int v2 = *(((int *) b) + 2);\n \n if (v1 > v2) return(-1);\n else if (v1 < v2) return(1);\n else return(0);\n}\n\nstatic void _sortPointList2(\n int *pointlist,\n int npoints){\n qsort(pointlist, npoints, 3*sizeof(int), _comparePoints2);\n}\n\nstatic float _minEigenvalue2(float gxx, float gxy, float gyy)\n{\n return (float) ((gxx + gyy - std::sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f);\n}\nvoid _enforceMinimumDistance2(uchar* featuremap,\n int *pointlist, /* featurepoints */\n int npoints, /* number of featurepoints */\n KLT_FeatureList featurelist, /* features */\n int ncols, int nrows, /* size of images */\n int mindist, /* min. dist b/w features */\n int min_eigenvalue, /* min. eigenvalue */\n bool overwriteAllFeatures)\n{\n int indx; /* Index into features */\n int x, y, val; /* Location and trackability of pixel under consideration */\n /* Boolean array recording proximity of features */\n int *ptr;\n \n /* Cannot add features with an eigenvalue less than one */\n if (min_eigenvalue < 1)\n min_eigenvalue = 1;\n \n //mindist--;\n /* Allocate memory for feature map and clear it */\n \n /*\n featuremap = (uchar *) malloc(ncols * nrows * sizeof(uchar));\n memset(featuremap, 0, ncols*nrows);\n \n \n \n if (!overwriteAllFeatures)\n for (indx = 0 ; indx < featurelist->nFeatures ; indx++)\n if (featurelist->feature[indx]->val >= 0) {\n x = (int) featurelist->feature[indx]->x;\n y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows);\n }\n */\n \n /* For each feature point, in descending order of importance, do ... */\n ptr = pointlist;\n indx = 0;\n while (1) {\n \n /* If we can't add all the points, then fill in the rest\n of the featurelist with -1's */\n if (ptr >= pointlist + 3*npoints) {\n while (indx < featurelist->nFeatures) {\n if (overwriteAllFeatures ||\n featurelist->feature[indx]->val < 0) {\n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = KLT_NOT_FOUND;\n \n /*featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_img_gradx = NULL;\n featurelist->feature[indx]->aff_img_grady = NULL;*/\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n }\n indx++;\n }\n break;\n }\n \n x = *ptr++;\n y = *ptr++;\n val = *ptr++;\n \n /* Ensure that feature is in-bounds */\n assert(x >= 0);\n assert(x < ncols);\n assert(y >= 0);\n assert(y < nrows);\n \n while (!overwriteAllFeatures &&\n indx < featurelist->nFeatures &&\n featurelist->feature[indx]->val >= 0)\n indx++;\n \n if (indx >= featurelist->nFeatures) break;\n \n /* If no neighbor has been selected, and if the minimum\n eigenvalue is large enough, then add feature to the current list */\n if (!featuremap[y*ncols+x] && val >= min_eigenvalue) {\n featurelist->feature[indx]->x = (KLT_locType) x;\n featurelist->feature[indx]->y = (KLT_locType) y;\n featurelist->feature[indx]->val = (int) val;\n \n /*featurelist->feature[indx]->aff_img = NULL;\n featurelist->feature[indx]->aff_gradx = NULL;\n featurelist->feature[indx]->aff_grady = NULL;*/\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n indx++;\n \n /* Fill in surrounding region of feature map, but\n make sure that pixels are in-bounds */\n _fillFeaturemap2(x, y, featuremap, mindist, ncols, nrows);\n }\n }\n \n /* Free feature map */\n //free(featuremap);\n}\n\nvoid _KLTSelectGoodFeatures(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList featurelist,\n selectionMode2 mode)\n{\n int npoints = 0;\n bool overwriteAllFeatures = (mode == SELECTING_ALL2) ? true : false;\n bool floatimages_created = false;\n \n int ncols=sumDxx.cols-1,nrows=sumDxx.rows-1;\n \n \n \n /* Check window size (and correct if necessary) */\n if (tc->window_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n \n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n \n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n \n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n int *pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));\n \n \n \n \n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n if (!overwriteAllFeatures){\n for (int indx = 0 ; indx < featurelist->nFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap2(x, y,featuremap,tc->mindist, ncols, nrows);\n }\n }\n }\n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n /* For most of the pixels in the image, do ... */\n ptr = pointlist;\n int nSkippedPixels=tc->nSkippedPixels;\n \n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n#ifdef USE_DILATE\n float* eigPtr=(float*)eig.ptr(y);\n#endif\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]==1) {\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float _val= _minEigenvalue2(gxx, gxy, gyy);\n int val;\n \n if (_val > limit) {\n val = limit;\n }else{\n val=(int)(_val+0.5);\n }\n \n if (val>tc->min_eigenvalue){\n#ifdef USE_DILATE\n eigPtr[x] = val;\n#else\n *ptr++ = x;\n *ptr++ = y;\n *ptr++ = val;\n npoints++;\n#endif\n }\n }\n }\n }\n //printf(\"%d noints\\n\",npoints);\n#ifdef USE_DILATE\n \n cv::Mat tmp;\n cv::dilate(eig,tmp,cv::Mat());\n cv::Size imgsize=tmp.size();\n \n std::vector tmpCorners;\n for( int y = 1; y < imgsize.height - 1; y++ )\n {\n const float* eig_data = (const float*)eig.ptr(y);\n const float* tmp_data = (const float*)tmp.ptr(y);\n \n for( int x = 1; x < imgsize.width - 1; x++ )\n {\n float val = eig_data[x];\n if( val != 0&& val == tmp_data[x])\n tmpCorners.push_back(eig_data + x);\n }\n }\n \n std::sort( tmpCorners.begin(), tmpCorners.end(), greaterThanPtr());\n \n npoints=tmpCorners.size();\n for (int i=0;imindist,\n tc->min_eigenvalue,\n overwriteAllFeatures);\n \n \n /* Free memory */\n free(featuremap);\n free(pointlist);\n}\n\n\n\nvoid KLTSelectGoodFeatures2(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL2);\n}\n\n\nvoid KLTReplaceLostFeatures2(KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n if (nLostFeatures > 0)\n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME2);\n \n}\n\n\n\nstruct greaterThanPtrInt:\npublic std::binary_function\n{\n bool operator () (const int * a, const int * b) const\n { return *a > *b; }\n};\n\nstruct smallerDistance:\npublic std::binary_function\n{\n bool operator () (const int a, const int b) const{\n \n float distanceY=a/sizeByGrid.width-float(sizeByGrid.height)/2.0;\n float distanceX=a%sizeByGrid.width-float(sizeByGrid.width)/2.0;\n \n float distanceA=distanceX*distanceX+distanceY*distanceY;\n \n distanceY=b/sizeByGrid.width-float(sizeByGrid.height)/2.0;\n distanceX=b%sizeByGrid.width-float(sizeByGrid.width)/2.0;\n \n float distanceB=distanceX*distanceX+distanceY*distanceY;\n \n return distanceA gridSequence;\n\nvoid initializeGridSequence(){\n gridSequence.resize(sizeByGrid.area());\n for(int i=0;iwindow_width % 2 != 1) {\n tc->window_width = tc->window_width+1;\n KLTWarning(\"Tracking context's window width must be odd. \"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height % 2 != 1) {\n tc->window_height = tc->window_height+1;\n KLTWarning(\"Tracking context's window height must be odd. \"\n \"Changing to %d.\\n\", tc->window_height);\n }\n if (tc->window_width < 3) {\n tc->window_width = 3;\n KLTWarning(\"Tracking context's window width must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_width);\n }\n if (tc->window_height < 3) {\n tc->window_height = 3;\n KLTWarning(\"Tracking context's window height must be at least three. \\n\"\n \"Changing to %d.\\n\", tc->window_height);\n }\n int window_hw = tc->window_width/2;\n int window_hh = tc->window_height/2;\n \n /* Create pointlist, which is a simplified version of a featurelist, */\n /* for speed. Contains only integer locations and values. */\n \n \n \n uchar *featuremap=(uchar*)malloc(ncols*nrows);\n memset(featuremap,0,ncols*nrows);\n \n \n //grid and eigmap\n std::vector > grids;\n grids.resize(sizeByGrid.width*sizeByGrid.height);\n for(int i=0;inFeatures ; indx++){\n if (featurelist->feature[indx]->val >= 0) {\n \n int x = (int) featurelist->feature[indx]->x;\n int y = (int) featurelist->feature[indx]->y;\n _fillFeaturemap(x,y,featuremap,tc->mindist, ncols, nrows);\n \n \n //register to grid\n int* eigPtr=(int*)eig.ptr(y);\n eigPtr[x]=(int)INT_MAX;\n \n int yByGrid=y/gridSize.height;\n int xByGrid=x/gridSize.width;\n grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]);\n }\n }\n }\n \n int nSkippedPixels=tc->nSkippedPixels;\n \n\n \n \n /* Compute trackability of each image pixel as the minimum\n of the two eigenvalues of the Z matrix */\n {\n float gxx, gxy, gyy;\n int *ptr;\n \n float val;\n unsigned int limit = 1;\n \n int borderx = tc->borderx;\t/* Must not touch cols */\n int bordery = tc->bordery;\t/* lost by convolution */\n \n if (borderx < window_hw){\n borderx = window_hw;\n }\n \n if (bordery < window_hh){\n bordery = window_hh;\n }\n \n /* Find largest value of an int */\n for (int i = 0 ; i < sizeof(int) ; i++){\n limit *= 256;\n }\n limit = limit/2 - 1;\n \n for (int y = bordery ; y < nrows - bordery ; y += nSkippedPixels + 1){\n \n float* dxyPtrs[2]={(float*)sumDxy.ptr(y-window_hh),(float*)sumDxy.ptr(y+window_hh+1)};\n float* dxxPtrs[2]={(float*)sumDxx.ptr(y-window_hh),(float*)sumDxx.ptr(y+window_hh+1)};\n float* dyyPtrs[2]={(float*)sumDyy.ptr(y-window_hh),(float*)sumDyy.ptr(y+window_hh+1)};\n \n int* eigPtr=(int*)eig.ptr(y);\n\n \n for (int x = borderx,x1=borderx-window_hw,x2=borderx+window_hw+1;\n x < ncols - borderx ;\n x += nSkippedPixels + 1,x1+=nSkippedPixels + 1,x2+=nSkippedPixels + 1) {\n \n if (featuremap[y*ncols+x]) {\n //assert(0);\n continue;\n }\n /* Sum the gradients in the surrounding window */\n gxy=dxyPtrs[0][x1]+dxyPtrs[1][x2]-dxyPtrs[1][x1]-dxyPtrs[0][x2];\n gxx=dxxPtrs[0][x1]+dxxPtrs[1][x2]-dxxPtrs[1][x1]-dxxPtrs[0][x2];\n gyy=dyyPtrs[0][x1]+dyyPtrs[1][x2]-dyyPtrs[1][x1]-dyyPtrs[0][x2];\n \n /* Store the trackability of the pixel as the minimum\n of the two eigenvalues */\n float val= _minEigenvalue(gxx, gxy, gyy);\n \n \n if (val > limit) {\n //assert(0);\n val = (float) limit;\n }\n \n if (val>tc->min_eigenvalue){\n eigPtr[x]=(int)val;\n int yByGrid=y/gridSize.height;\n int xByGrid=x/gridSize.width;\n grids[yByGrid*sizeByGrid.width+xByGrid].push_back(&eigPtr[x]);\n }\n }\n }\n }\n \n \n for(int i=0;imaxFeatureInGrid&&val!=INT_MAX){\n break;\n }*/\n\n \n if(val!=INT_MAX){\n \n int ofs = (int)((const uchar*)grids[i][j] - eig.ptr());\n int y = (int)(ofs / eig.step);\n int x = (int)((ofs-y*eig.step)/sizeof(int));\n \n \n while (!overwriteAllFeatures &&indx < featurelist->nFeatures&& featurelist->feature[indx]->val >= 0){\n indx++;\n }\n \n if (indx >= featurelist->nFeatures){\n if(featureAdded>minFeatureInGrid){\n break;\n }else{\n \n featurelist->nFeatures++;\n \n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = -1;\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n \n }\n }\n \n \n if (!featuremap[y*ncols+x]) {\n \n featurelist->feature[indx]->x = (KLT_locType) x;\n featurelist->feature[indx]->y = (KLT_locType) y;\n featurelist->feature[indx]->val = (int) val;\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n indx++;\n \n _fillFeaturemap(x, y, featuremap,tc->mindist, ncols, nrows);\n featureAdded++;\n }\n }\n }\n }\n \n \n while (indx < featurelist->nFeatures) {\n \n if (overwriteAllFeatures || featurelist->feature[indx]->val < 0) {\n \n featurelist->feature[indx]->x = -1;\n featurelist->feature[indx]->y = -1;\n featurelist->feature[indx]->val = KLT_NOT_FOUND;\n \n featurelist->feature[indx]->aff_x = -1.0;\n featurelist->feature[indx]->aff_y = -1.0;\n featurelist->feature[indx]->aff_Axx = 1.0;\n featurelist->feature[indx]->aff_Ayx = 0.0;\n featurelist->feature[indx]->aff_Axy = 0.0;\n featurelist->feature[indx]->aff_Ayy = 1.0;\n }\n indx++;\n }\n \n free(featuremap);\n \n}\n\nvoid KLTSelectGoodFeaturesUniform(\n KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n \n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n \n sizeByGrid.width=ncols/gridSize.width;\n sizeByGrid.height=nrows/gridSize.height;\n \n minFeatureInGrid=int((fl->nFeatures/float(sizeByGrid.area()))+0.5);\n initializeGridSequence();\n \n \n _KLTSelectGoodFeaturesUniform(tc,sumDxx,sumDyy,sumDxy,fl, SELECTING_ALL);\n}\n\n\nvoid KLTReplaceLostFeaturesUniform(\n KLT_TrackingContext tc,\n const cv::Mat& sumDxx,\n const cv::Mat& sumDyy,\n const cv::Mat& sumDxy,\n KLT_FeatureList fl){\n \n int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);\n int ncols=sumDxx.cols-1;\n int nrows=sumDxx.rows-1;\n \n if (nLostFeatures > 0)\n _KLTSelectGoodFeatures(tc,sumDxx,sumDyy,sumDxy,fl,REPLACING_SOME);\n \n}\n\n\n\n\n\n\n\n\n\n" }, { "path": "GSLAM/svdlib.cpp", "content": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n* Neither the name of the University of Tennessee nor the names of its\n contributors may be used to endorse or promote products derived from this\n software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n*/\n\n#include \n#include \n#include \"svdlib.hpp\"\n#include \"svdutil.hpp\"\n\nchar *SVDVersion = \"1.4\";\nlong SVDVerbosity = 0;\nlong SVDCount[SVD_COUNTERS];\n\nvoid svdResetCounters(void) {\n int i;\n for (i = 0; i < SVD_COUNTERS; i++)\n SVDCount[i] = 0;\n}\n\n/********************************* Allocation ********************************/\n\n/* Row major order. Rows are vectors that are consecutive in memory. Matrix\n is initialized to empty. */\nDMat svdNewDMat(int rows, int cols) {\n int i;\n DMat D = (DMat) malloc(sizeof(struct dmat));\n if (!D) {perror(\"svdNewDMat\"); return NULL;}\n D->rows = rows;\n D->cols = cols;\n\n D->value = (double **) malloc(rows * sizeof(double *));\n if (!D->value) {SAFE_FREE(D); return NULL;}\n\n D->value[0] = (double *) calloc(rows * cols, sizeof(double));\n if (!D->value[0]) {SAFE_FREE(D->value); SAFE_FREE(D); return NULL;}\n\n for (i = 1; i < rows; i++) D->value[i] = D->value[i-1] + cols;\n return D;\n}\n\nvoid svdFreeDMat(DMat D) {\n if (!D) return;\n SAFE_FREE(D->value[0]);\n SAFE_FREE(D->value);\n free(D);\n}\n\n\nSMat svdNewSMat(int rows, int cols, int vals) {\n SMat S = (SMat) calloc(1, sizeof(struct smat));\n if (!S) {perror(\"svdNewSMat\"); return NULL;}\n S->rows = rows;\n S->cols = cols;\n S->vals = vals;\n S->pointr = svd_longArray(cols + 1, TRUE, \"svdNewSMat: pointr\");\n if (!S->pointr) {svdFreeSMat(S); return NULL;}\n S->rowind = svd_longArray(vals, FALSE, \"svdNewSMat: rowind\");\n if (!S->rowind) {svdFreeSMat(S); return NULL;}\n S->value = svd_doubleArray(vals, FALSE, \"svdNewSMat: value\");\n if (!S->value) {svdFreeSMat(S); return NULL;}\n return S;\n}\n\nvoid svdFreeSMat(SMat S) {\n if (!S) return;\n SAFE_FREE(S->pointr);\n SAFE_FREE(S->rowind);\n SAFE_FREE(S->value);\n free(S);\n}\n\n\n/* Creates an empty SVD record */\nSVDRec svdNewSVDRec(void) {\n SVDRec R = (SVDRec) calloc(1, sizeof(struct svdrec));\n if (!R) {perror(\"svdNewSVDRec\"); return NULL;}\n return R;\n}\n\n/* Frees an svd rec and all its contents. */\nvoid svdFreeSVDRec(SVDRec R) {\n if (!R) return;\n if (R->Ut) svdFreeDMat(R->Ut);\n if (R->S) SAFE_FREE(R->S);\n if (R->Vt) svdFreeDMat(R->Vt);\n free(R);\n}\n\n\n/**************************** Conversion *************************************/\n\n/* Converts a sparse matrix to a dense one (without affecting the former) */\nDMat svdConvertStoD(SMat S) {\n int i, c;\n DMat D = svdNewDMat(S->rows, S->cols);\n if (!D) {\n svd_error(\"svdConvertStoD: failed to allocate D\");\n return NULL;\n }\n for (i = 0, c = 0; i < S->vals; i++) {\n while (S->pointr[c + 1] <= i) c++;\n D->value[S->rowind[i]][c] = S->value[i];\n }\n return D;\n}\n\n/* Converts a dense matrix to a sparse one (without affecting the dense one) */\nSMat svdConvertDtoS(DMat D) {\n SMat S;\n int i, j, n;\n for (i = 0, n = 0; i < D->rows; i++)\n for (j = 0; j < D->cols; j++)\n if (D->value[i][j] != 0) n++;\n \n S = svdNewSMat(D->rows, D->cols, n);\n if (!S) {\n svd_error(\"svdConvertDtoS: failed to allocate S\");\n return NULL;\n }\n for (j = 0, n = 0; j < D->cols; j++) {\n S->pointr[j] = n;\n for (i = 0; i < D->rows; i++)\n if (D->value[i][j] != 0) {\n S->rowind[n] = i;\n S->value[n] = D->value[i][j];\n n++;\n }\n }\n S->pointr[S->cols] = S->vals;\n return S;\n}\n\n/* Transposes a dense matrix. */\nDMat svdTransposeD(DMat D) {\n int r, c;\n DMat N = svdNewDMat(D->cols, D->rows);\n for (r = 0; r < D->rows; r++)\n for (c = 0; c < D->cols; c++)\n N->value[c][r] = D->value[r][c];\n return N;\n}\n\n/* Efficiently transposes a sparse matrix. */\nSMat svdTransposeS(SMat S) {\n int r, c, i, j;\n SMat N = svdNewSMat(S->cols, S->rows, S->vals);\n /* Count number nz in each row. */\n for (i = 0; i < S->vals; i++)\n N->pointr[S->rowind[i]]++;\n /* Fill each cell with the starting point of the previous row. */\n N->pointr[S->rows] = S->vals - N->pointr[S->rows - 1];\n for (r = S->rows - 1; r > 0; r--)\n N->pointr[r] = N->pointr[r+1] - N->pointr[r-1];\n N->pointr[0] = 0;\n /* Assign the new columns and values. */\n for (c = 0, i = 0; c < S->cols; c++) {\n for (; i < S->pointr[c+1]; i++) {\n r = S->rowind[i];\n j = N->pointr[r+1]++;\n N->rowind[j] = c;\n N->value[j] = S->value[i];\n }\n }\n return N;\n}\n\n\n/**************************** Input/Output ***********************************/\n\nvoid svdWriteDenseArray(double *a, int n, char *filename, char binary) {\n int i;\n FILE *file = svd_writeFile(filename, FALSE);\n if (!file) \n return svd_error(\"svdWriteDenseArray: failed to write %s\", filename);\n if (binary) {\n svd_writeBinInt(file, n);\n for (i = 0; i < n; i++)\n svd_writeBinFloat(file, (float) a[i]);\n } else {\n fprintf(file, \"%d\\n\", n);\n for (i = 0; i < n; i++)\n fprintf(file, \"%g\\n\", a[i]);\n }\n svd_closeFile(file);\n}\n\ndouble *svdLoadDenseArray(char *filename, int *np, char binary) {\n int i, n;\n double *a;\n\n FILE *file = svd_readFile(filename);\n if (!file) {\n svd_error(\"svdLoadDenseArray: failed to read %s\", filename);\n return NULL;\n }\n if (binary) {\n svd_readBinInt(file, np);\n } else if (fscanf(file, \" %d\", np) != 1) {\n svd_error(\"svdLoadDenseArray: error reading %s\", filename);\n svd_closeFile(file);\n return NULL; \n }\n n = *np;\n a = svd_doubleArray(n, FALSE, \"svdLoadDenseArray: a\");\n if (!a) return NULL;\n if (binary) {\n float f;\n for (i = 0; i < n; i++) {\n svd_readBinFloat(file, &f);\n a[i] = f;\n }\n } else {\n for (i = 0; i < n; i++) {\n if (fscanf(file, \" %lf\\n\", a + i) != 1) {\n\tsvd_error(\"svdLoadDenseArray: error reading %s\", filename);\n\tbreak;\n }\n }\n }\n svd_closeFile(file);\n return a;\n}\n\n\n/* File format has a funny header, then first entry index per column, then the\n row for each entry, then the value for each entry. Indices count from 1.\n Assumes A is initialized. */\nstatic SMat svdLoadSparseTextHBFile(FILE *file) {\n char line[128];\n long i, x, rows, cols, vals, num_mat;\n SMat S;\n /* Skip the header line: */\n if (!fgets(line, 128, file));\n /* Skip the line giving the number of lines in this file: */\n if (!fgets(line, 128, file));\n /* Read the line with useful dimensions: */\n if (fscanf(file, \"%*s%ld%ld%ld%ld\\n\", \n &rows, &cols, &vals, &num_mat) != 4) {\n svd_error(\"svdLoadSparseTextHBFile: bad file format on line 3\");\n return NULL;\n }\n if (num_mat != 0) {\n svd_error(\"svdLoadSparseTextHBFile: I don't know how to handle a file \"\n \"with elemental matrices (last entry on header line 3)\");\n return NULL;\n }\n /* Skip the line giving the formats: */\n if (!fgets(line, 128, file));\n \n S = svdNewSMat(rows, cols, vals);\n if (!S) return NULL;\n \n /* Read column pointers. */\n for (i = 0; i <= S->cols; i++) {\n if (fscanf(file, \" %ld\", &x) != 1) {\n svd_error(\"svdLoadSparseTextHBFile: error reading pointr %d\", i);\n return NULL;\n }\n S->pointr[i] = x - 1;\n }\n S->pointr[S->cols] = S->vals;\n \n /* Read row indices. */\n for (i = 0; i < S->vals; i++) {\n if (fscanf(file, \" %ld\", &x) != 1) {\n svd_error(\"svdLoadSparseTextHBFile: error reading rowind %d\", i);\n return NULL;\n }\n S->rowind[i] = x - 1;\n }\n for (i = 0; i < S->vals; i++) \n if (fscanf(file, \" %lf\", S->value + i) != 1) {\n svd_error(\"svdLoadSparseTextHBFile: error reading value %d\", i);\n return NULL;\n }\n return S;\n}\n\nstatic void svdWriteSparseTextHBFile(SMat S, FILE *file) {\n int i;\n long col_lines = ((S->cols + 1) / 8) + (((S->cols + 1) % 8) ? 1 : 0);\n long row_lines = (S->vals / 8) + ((S->vals % 8) ? 1 : 0);\n long total_lines = col_lines + 2 * row_lines;\n \n char title[32];\n sprintf(title, \"SVDLIBC v. %s\", SVDVersion);\n fprintf(file, \"%-72s%-8s\\n\", title, \"\");\n fprintf(file, \"%14ld%14ld%14ld%14ld%14d\\n\", total_lines, col_lines,\n row_lines, row_lines, 0);\n fprintf(file, \"%-14s%14ld%14ld%14ld%14d\\n\", \"rra\", S->rows, S->cols,\n S->vals, 0);\n fprintf(file, \"%16s%16s%16s%16s\\n\", \"(8i)\", \"(8i)\", \"(8e)\", \"(8e)\");\n\n for (i = 0; i <= S->cols; i++)\n fprintf(file, \"%ld%s\", S->pointr[i] + 1, (((i+1) % 8) == 0) ? \"\\n\" : \" \");\n fprintf(file, \"\\n\");\n for (i = 0; i < S->vals; i++)\n fprintf(file, \"%ld%s\", S->rowind[i] + 1, (((i+1) % 8) == 0) ? \"\\n\" : \" \");\n fprintf(file, \"\\n\");\n for (i = 0; i < S->vals; i++)\n fprintf(file, \"%g%s\", S->value[i], (((i+1) % 8) == 0) ? \"\\n\" : \" \");\n fprintf(file, \"\\n\");\n}\n\n\nstatic SMat svdLoadSparseTextFile(FILE *file) {\n long c, i, n, v, rows, cols, vals;\n SMat S;\n if (fscanf(file, \" %ld %ld %ld\", &rows, &cols, &vals) != 3) {\n svd_error(\"svdLoadSparseTextFile: bad file format\");\n return NULL;\n }\n\n S = svdNewSMat(rows, cols, vals);\n if (!S) return NULL;\n \n for (c = 0, v = 0; c < cols; c++) {\n if (fscanf(file, \" %ld\", &n) != 1) {\n svd_error(\"svdLoadSparseTextFile: bad file format\");\n return NULL;\n }\n S->pointr[c] = v;\n for (i = 0; i < n; i++, v++) {\n if (fscanf(file, \" %ld %lf\", S->rowind + v, S->value + v) != 2) {\n svd_error(\"svdLoadSparseTextFile: bad file format\");\n return NULL;\n }\n }\n }\n S->pointr[cols] = vals;\n return S;\n}\n\nstatic void svdWriteSparseTextFile(SMat S, FILE *file) {\n int c, v;\n fprintf(file, \"%ld %ld %ld\\n\", S->rows, S->cols, S->vals);\n for (c = 0, v = 0; c < S->cols; c++) {\n fprintf(file, \"%ld\\n\", S->pointr[c + 1] - S->pointr[c]);\n for (; v < S->pointr[c+1]; v++)\n fprintf(file, \"%ld %g\\n\", S->rowind[v], S->value[v]);\n }\n}\n\n\nstatic SMat svdLoadSparseBinaryFile(FILE *file) {\n int rows, cols, vals, n, c, i, v, r, e = 0;\n float f;\n SMat S;\n e += svd_readBinInt(file, &rows);\n e += svd_readBinInt(file, &cols);\n e += svd_readBinInt(file, &vals);\n if (e) {\n svd_error(\"svdLoadSparseBinaryFile: bad file format\");\n return NULL;\n }\n\n S = svdNewSMat(rows, cols, vals);\n if (!S) return NULL;\n \n for (c = 0, v = 0; c < cols; c++) {\n if (svd_readBinInt(file, &n)) {\n svd_error(\"svdLoadSparseBinaryFile: bad file format\");\n return NULL;\n }\n S->pointr[c] = v;\n for (i = 0; i < n; i++, v++) {\n e += svd_readBinInt(file, &r);\n e += svd_readBinFloat(file, &f);\n if (e) {\n svd_error(\"svdLoadSparseBinaryFile: bad file format\");\n return NULL;\n }\n S->rowind[v] = r;\n S->value[v] = f;\n }\n }\n S->pointr[cols] = vals;\n return S;\n}\n\nstatic void svdWriteSparseBinaryFile(SMat S, FILE *file) {\n int c, v;\n svd_writeBinInt(file, (int) S->rows);\n svd_writeBinInt(file, (int) S->cols);\n svd_writeBinInt(file, (int) S->vals);\n for (c = 0, v = 0; c < S->cols; c++) {\n svd_writeBinInt(file, (int) (S->pointr[c + 1] - S->pointr[c]));\n for (; v < S->pointr[c+1]; v++) {\n svd_writeBinInt(file, (int) S->rowind[v]);\n svd_writeBinFloat(file, (float) S->value[v]);\n }\n }\n}\n\n\nstatic DMat svdLoadDenseTextFile(FILE *file) {\n long rows, cols, i, j;\n DMat D;\n if (fscanf(file, \" %ld %ld\", &rows, &cols) != 2) {\n svd_error(\"svdLoadDenseTextFile: bad file format\");\n return NULL;\n }\n\n D = svdNewDMat(rows, cols);\n if (!D) return NULL;\n\n for (i = 0; i < rows; i++)\n for (j = 0; j < cols; j++) {\n if (fscanf(file, \" %lf\", &(D->value[i][j])) != 1) {\n svd_error(\"svdLoadDenseTextFile: bad file format\");\n return NULL;\n }\n }\n return D;\n}\n\nstatic void svdWriteDenseTextFile(DMat D, FILE *file) {\n int i, j;\n fprintf(file, \"%ld %ld\\n\", D->rows, D->cols);\n for (i = 0; i < D->rows; i++)\n for (j = 0; j < D->cols; j++) \n fprintf(file, \"%g%c\", D->value[i][j], (j == D->cols - 1) ? '\\n' : ' ');\n}\n\n\nstatic DMat svdLoadDenseBinaryFile(FILE *file) {\n int rows, cols, i, j, e = 0;\n float f;\n DMat D;\n e += svd_readBinInt(file, &rows);\n e += svd_readBinInt(file, &cols);\n if (e) {\n svd_error(\"svdLoadDenseBinaryFile: bad file format\");\n return NULL;\n }\n\n D = svdNewDMat(rows, cols);\n if (!D) return NULL;\n\n for (i = 0; i < rows; i++)\n for (j = 0; j < cols; j++) {\n if (svd_readBinFloat(file, &f)) {\n svd_error(\"svdLoadDenseBinaryFile: bad file format\");\n return NULL;\n }\n D->value[i][j] = f;\n }\n return D;\n}\n\nstatic void svdWriteDenseBinaryFile(DMat D, FILE *file) {\n int i, j;\n svd_writeBinInt(file, (int) D->rows);\n svd_writeBinInt(file, (int) D->cols);\n for (i = 0; i < D->rows; i++)\n for (j = 0; j < D->cols; j++) \n svd_writeBinFloat(file, (float) D->value[i][j]);\n}\n\n\nSMat svdLoadSparseMatrix(char *filename, int format) {\n SMat S = NULL;\n DMat D = NULL;\n FILE *file = svd_fatalReadFile(filename);\n switch (format) {\n case SVD_F_STH: \n S = svdLoadSparseTextHBFile(file);\n break;\n case SVD_F_ST:\n S = svdLoadSparseTextFile(file);\n break;\n case SVD_F_SB:\n S = svdLoadSparseBinaryFile(file);\n break;\n case SVD_F_DT:\n D = svdLoadDenseTextFile(file);\n break;\n case SVD_F_DB:\n D = svdLoadDenseBinaryFile(file);\n break;\n default: svd_error(\"svdLoadSparseMatrix: unknown format %d\", format);\n }\n svd_closeFile(file);\n if (D) {\n S = svdConvertDtoS(D);\n svdFreeDMat(D);\n }\n return S;\n}\n\nDMat svdLoadDenseMatrix(char *filename, int format) {\n SMat S = NULL;\n DMat D = NULL;\n FILE *file = svd_fatalReadFile(filename);\n switch (format) {\n case SVD_F_STH: \n S = svdLoadSparseTextHBFile(file);\n break;\n case SVD_F_ST:\n S = svdLoadSparseTextFile(file);\n break;\n case SVD_F_SB:\n S = svdLoadSparseBinaryFile(file);\n break;\n case SVD_F_DT:\n D = svdLoadDenseTextFile(file);\n break;\n case SVD_F_DB:\n D = svdLoadDenseBinaryFile(file);\n break;\n default: svd_error(\"svdLoadSparseMatrix: unknown format %d\", format);\n }\n svd_closeFile(file);\n if (S) {\n D = svdConvertStoD(S);\n svdFreeSMat(S);\n }\n return D;\n}\n\nvoid svdWriteSparseMatrix(SMat S, char *filename, int format) {\n DMat D = NULL;\n FILE *file = svd_writeFile(filename, FALSE);\n if (!file) {\n svd_error(\"svdWriteSparseMatrix: failed to write file %s\\n\", filename);\n return;\n }\n switch (format) {\n case SVD_F_STH: \n svdWriteSparseTextHBFile(S, file);\n break;\n case SVD_F_ST:\n svdWriteSparseTextFile(S, file);\n break;\n case SVD_F_SB:\n svdWriteSparseBinaryFile(S, file);\n break;\n case SVD_F_DT:\n D = svdConvertStoD(S);\n svdWriteDenseTextFile(D, file);\n break;\n case SVD_F_DB:\n D = svdConvertStoD(S);\n svdWriteDenseBinaryFile(D, file);\n break;\n default: svd_error(\"svdLoadSparseMatrix: unknown format %d\", format);\n }\n svd_closeFile(file);\n if (D) svdFreeDMat(D);\n}\n\nvoid svdWriteDenseMatrix(DMat D, char *filename, int format) {\n SMat S = NULL;\n FILE *file = svd_writeFile(filename, FALSE);\n if (!file) {\n svd_error(\"svdWriteDenseMatrix: failed to write file %s\\n\", filename);\n return;\n }\n switch (format) {\n case SVD_F_STH: \n S = svdConvertDtoS(D);\n svdWriteSparseTextHBFile(S, file);\n break;\n case SVD_F_ST:\n S = svdConvertDtoS(D);\n svdWriteSparseTextFile(S, file);\n break;\n case SVD_F_SB:\n S = svdConvertDtoS(D);\n svdWriteSparseBinaryFile(S, file);\n break;\n case SVD_F_DT:\n svdWriteDenseTextFile(D, file);\n break;\n case SVD_F_DB:\n svdWriteDenseBinaryFile(D, file);\n break;\n default: svd_error(\"svdLoadSparseMatrix: unknown format %d\", format);\n }\n svd_closeFile(file);\n if (S) svdFreeSMat(S);\n}\n" }, { "path": "GSLAM/svdlib.hpp", "content": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n* Neither the name of the University of Tennessee nor the names of its\n contributors may be used to endorse or promote products derived from this\n software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n*/\n\n#ifndef SVDLIB_H\n#define SVDLIB_H\n\n#ifndef FALSE\n# define FALSE 0\n#endif\n#ifndef TRUE\n# define TRUE 1\n#endif\n\n/******************************** Structures *********************************/\ntypedef struct smat *SMat;\ntypedef struct dmat *DMat;\ntypedef struct svdrec *SVDRec;\n\n\nstatic SVDRec globalSVDRec;\nconst bool useInitialization=true;\n\n/* Harwell-Boeing sparse matrix. */\nstruct smat {\n long rows;\n long cols;\n long vals; /* Total non-zero entries. */\n long *pointr; /* For each col (plus 1), index of first non-zero entry. */\n long *rowind; /* For each nz entry, the row index. */\n double *value; /* For each nz entry, the value. */\n};\n\n/* Row-major dense matrix. Rows are consecutive vectors. */\nstruct dmat {\n long rows;\n long cols;\n double **value; /* Accessed by [row][col]. Free value[0] and value to free.*/\n};\n\nstruct svdrec {\n int d; /* Dimensionality (rank) */\n DMat Ut; /* Transpose of left singular vectors. (d by m)\n The vectors are the rows of Ut. */\n double *S; /* Array of singular values. (length d) */\n DMat Vt; /* Transpose of right singular vectors. (d by n)\n The vectors are the rows of Vt. */\n};\n\n\n/******************************** Variables **********************************/\n\n/* Version info */\nextern char *SVDVersion;\n\n/* How verbose is the package: 0, 1 (default), 2 */\nextern long SVDVerbosity;\n\n/* Counter(s) used to track how much work is done in computing the SVD. */\nenum svdCounters {SVD_MXV, SVD_COUNTERS};\nextern long SVDCount[SVD_COUNTERS];\nextern void svdResetCounters(void);\n\nenum svdFileFormats {SVD_F_STH, SVD_F_ST, SVD_F_SB, SVD_F_DT, SVD_F_DB};\n/*\nFile formats:\nSVD_F_STH: sparse text, SVDPACK-style\nSVD_F_ST: sparse text, SVDLIB-style\nSVD_F_DT: dense text\nSVD_F_SB: sparse binary\nSVD_F_DB: dense binary\n*/\n\n/* True if a file format is sparse: */\n#define SVD_IS_SPARSE(format) ((format >= SVD_F_STH) && (format <= SVD_F_SB))\n\n\n/******************************** Functions **********************************/\n\n/* Creates an empty dense matrix. */\nextern DMat svdNewDMat(int rows, int cols);\n/* Frees a dense matrix. */\nextern void svdFreeDMat(DMat D);\n\n/* Creates an empty sparse matrix. */\nSMat svdNewSMat(int rows, int cols, int vals);\n/* Frees a sparse matrix. */\nvoid svdFreeSMat(SMat S);\n\n/* Creates an empty SVD record. */\nSVDRec svdNewSVDRec(void);\n/* Frees an svd rec and all its contents. */\nvoid svdFreeSVDRec(SVDRec R);\n\n/* Converts a sparse matrix to a dense one (without affecting former) */\nDMat svdConvertStoD(SMat S);\n/* Converts a dense matrix to a sparse one (without affecting former) */\nSMat svdConvertDtoS(DMat D);\n\n/* Transposes a dense matrix (returning a new one) */\nDMat svdTransposeD(DMat D);\n/* Transposes a sparse matrix (returning a new one) */\nSMat svdTransposeS(SMat S);\n\n/* Writes an array to a file. */\nextern void svdWriteDenseArray(double *a, int n, char *filename, char binary);\n/* Reads an array from a file, storing its size in *np. */\nextern double *svdLoadDenseArray(char *filename, int *np, char binary);\n\n/* Loads a matrix file (in various formats) into a sparse matrix. */\nextern SMat svdLoadSparseMatrix(char *filename, int format);\n/* Loads a matrix file (in various formats) into a dense matrix. */\nextern DMat svdLoadDenseMatrix(char *filename, int format);\n\n/* Writes a dense matrix to a file in a given format. */\nextern void svdWriteDenseMatrix(DMat A, char *filename, int format);\n/* Writes a sparse matrix to a file in a given format. */\nextern void svdWriteSparseMatrix(SMat A, char *filename, int format);\n\n\n/* Performs the las2 SVD algorithm and returns the resulting Ut, S, and Vt. */\nextern SVDRec svdLAS2(SMat A, long dimensions, long iterations, double end[2], \n double kappa);\n/* Chooses default parameter values. Set dimensions to 0 for all dimensions: */\nextern SVDRec svdLAS2A(SMat A, long dimensions);\n\n#endif /* SVDLIB_H */\n" }, { "path": "GSLAM/svdutil.cpp", "content": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n* Neither the name of the University of Tennessee nor the names of its\n contributors may be used to endorse or promote products derived from this\n software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n*/\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \"svdlib.hpp\"\n#include \"svdutil.hpp\"\n\n#define BUNZIP2 \"bzip2 -d\"\n#define BZIP2 \"bzip2 -1\"\n#define UNZIP \"gzip -d\"\n#define ZIP \"gzip -1\"\n#define COMPRESS \"compress\"\n\n#define MAX_FILENAME 512\n#define MAX_PIPES 64\nstatic FILE *Pipe[MAX_PIPES];\nstatic int numPipes = 0;\n\nlong *svd_longArray(long size, char empty, char *name) {\n long *a;\n if (empty) a = (long *) calloc(size, sizeof(long));\n else a = (long *) malloc(size * sizeof(long));\n if (!a) {\n perror(name);\n /* exit(errno); */\n }\n return a;\n}\n\ndouble *svd_doubleArray(long size, char empty, char *name) {\n double *a;\n if (empty) a = (double *) calloc(size, sizeof(double));\n else a = (double *) malloc(size * sizeof(double));\n if (!a) {\n perror(name);\n /* exit(errno); */\n }\n return a;\n}\n\nvoid svd_beep(void) {\n fputc('\\a', stderr);\n fflush(stderr);\n}\n\nvoid svd_debug(char *fmt, ...) {\n va_list args;\n va_start(args, fmt);\n vfprintf(stderr, fmt, args);\n va_end(args);\n}\n\nvoid svd_error(char *fmt, ...) {\n va_list args;\n va_start(args, fmt);\n svd_beep();\n fprintf(stderr, \"ERROR: \");\n vfprintf(stderr, fmt, args);\n fprintf(stderr, \"\\n\");\n va_end(args);\n}\n\nvoid svd_fatalError(char *fmt, ...) {\n va_list args;\n va_start(args, fmt);\n svd_beep();\n fprintf(stderr, \"ERROR: \");\n vfprintf(stderr, fmt, args);\n fprintf(stderr, \"\\a\\n\");\n va_end(args);\n exit(1);\n}\n\nstatic void registerPipe(FILE *p) {\n if (numPipes >= MAX_PIPES) svd_error(\"Too many pipes open\");\n Pipe[numPipes++] = p;\n}\n\nstatic char isPipe(FILE *p) {\n int i;\n for (i = 0; i < numPipes && Pipe[i] != p; i++);\n if (i == numPipes) return FALSE;\n Pipe[i] = Pipe[--numPipes];\n return TRUE;\n}\n\nstatic FILE *openPipe(char *pipeName, char *mode) {\n FILE *pipe;\n fflush(stdout);\n if ((pipe = popen(pipeName, mode))) registerPipe(pipe);\n return pipe;\n}\n\nstatic FILE *readZippedFile(char *command, char *fileName) {\n char buf[MAX_FILENAME];\n sprintf(buf, \"%s < %s 2>/dev/null\", command, fileName);\n return openPipe(buf, \"r\");\n}\n\nFILE *svd_fatalReadFile(char *filename) {\n FILE *file;\n if (!(file = svd_readFile(filename)))\n svd_fatalError(\"couldn't read the file %s\", filename);\n return file;\n}\n\nstatic int stringEndsIn(char *s, char *t) {\n int ls = strlen(s);\n int lt = strlen(t);\n if (ls < lt) return FALSE;\n return (strcmp(s + ls - lt, t)) ? FALSE : TRUE;\n}\n\n/* Will silently return NULL if file couldn't be opened */\nFILE *svd_readFile(char *fileName) {\n char fileBuf[MAX_FILENAME];\n struct stat statbuf;\n\n /* Special file name */\n if (!strcmp(fileName, \"-\"))\n return stdin;\n \n /* If it is a pipe */\n if (fileName[0] == '|')\n return openPipe(fileName + 1, \"r\");\n\n /* Check if already ends in .gz or .Z and assume compressed */\n if (stringEndsIn(fileName, \".gz\") || stringEndsIn(fileName, \".Z\")) {\n if (!stat(fileName, &statbuf))\n return readZippedFile(UNZIP, fileName);\n return NULL;\n }\n /* Check if already ends in .bz or .bz2 and assume compressed */\n if (stringEndsIn(fileName, \".bz\") || stringEndsIn(fileName, \".bz2\")) {\n if (!stat(fileName, &statbuf))\n return readZippedFile(BUNZIP2, fileName);\n return NULL;\n }\n /* Try just opening normally */\n if (!stat(fileName, &statbuf))\n return fopen(fileName, \"r\");\n /* Try adding .gz */\n sprintf(fileBuf, \"%s.gz\", fileName);\n if (!stat(fileBuf, &statbuf))\n return readZippedFile(UNZIP, fileBuf);\n /* Try adding .Z */\n sprintf(fileBuf, \"%s.Z\", fileName);\n if (!stat(fileBuf, &statbuf))\n return readZippedFile(UNZIP, fileBuf);\n /* Try adding .bz2 */\n sprintf(fileBuf, \"%s.bz2\", fileName);\n if (!stat(fileBuf, &statbuf))\n return readZippedFile(BUNZIP2, fileBuf);\n /* Try adding .bz */\n sprintf(fileBuf, \"%s.bz\", fileName);\n if (!stat(fileBuf, &statbuf))\n return readZippedFile(BUNZIP2, fileBuf);\n\n return NULL;\n}\n\nstatic FILE *writeZippedFile(char *fileName, char append) {\n char buf[MAX_FILENAME];\n const char *op = (append) ? \">>\" : \">\";\n if (stringEndsIn(fileName, \".bz2\") || stringEndsIn(fileName, \".bz\"))\n sprintf(buf, \"%s %s \\\"%s\\\"\", BZIP2, op, fileName);\n else if (stringEndsIn(fileName, \".Z\"))\n sprintf(buf, \"%s %s \\\"%s\\\"\", COMPRESS, op, fileName);\n else\n sprintf(buf, \"%s %s \\\"%s\\\"\", ZIP, op, fileName);\n return openPipe(buf, \"w\");\n}\n\nFILE *svd_writeFile(char *fileName, char append) {\n /* Special file name */\n if (!strcmp(fileName, \"-\"))\n return stdout;\n \n /* If it is a pipe */\n if (fileName[0] == '|')\n return openPipe(fileName + 1, \"w\");\n\n /* Check if ends in .gz, .Z, .bz, .bz2 */\n if (stringEndsIn(fileName, \".gz\") || stringEndsIn(fileName, \".Z\") ||\n stringEndsIn(fileName, \".bz\") || stringEndsIn(fileName, \".bz2\"))\n return writeZippedFile(fileName, append);\n return (append) ? fopen(fileName, \"a\") : fopen(fileName, \"w\");\n}\n\n/* Could be a file or a stream. */\nvoid svd_closeFile(FILE *file) {\n if (file == stdin || file == stdout) return;\n if (isPipe(file)) pclose(file);\n else fclose(file);\n}\n\n\nchar svd_readBinInt(FILE *file, int *val) {\n int x;\n if (fread(&x, sizeof(int), 1, file) == 1) {\n *val = ntohl(x);\n return FALSE;\n }\n return TRUE;\n}\n\n/* This reads a float in network order and converts to a real in host order. */\nchar svd_readBinFloat(FILE *file, float *val) {\n int x;\n float y;\n if (fread(&x, sizeof(int), 1, file) == 1) {\n x = ntohl(x);\n y = *((float *) &x);\n *val = y;\n return FALSE;\n }\n return TRUE;\n}\n\nchar svd_writeBinInt(FILE *file, int x) {\n int y = htonl(x);\n if (fwrite(&y, sizeof(int), 1, file) != 1) return TRUE;\n return FALSE;\n}\n\n/* This takes a real in host order and writes a float in network order. */\nchar svd_writeBinFloat(FILE *file, float r) {\n int y = htonl(*((int *) &r));\n if (fwrite(&y, sizeof(int), 1, file) != 1) return TRUE;\n return FALSE;\n}\n\n\n\n/************************************************************** \n * returns |a| if b is positive; else fsign returns -|a| *\n **************************************************************/ \ndouble svd_fsign(double a, double b) {\n if ((a>=0.0 && b>=0.0) || (a<0.0 && b<0.0))return(a);\n else return -a;\n}\n\n/************************************************************** \n * returns the larger of two double precision numbers *\n **************************************************************/ \ndouble svd_dmax(double a, double b) {\n return (a > b) ? a : b;\n}\n\n/************************************************************** \n * returns the smaller of two double precision numbers *\n **************************************************************/ \ndouble svd_dmin(double a, double b) {\n return (a < b) ? a : b;\n}\n\n/************************************************************** \n * returns the larger of two integers *\n **************************************************************/ \nlong svd_imax(long a, long b) {\n return (a > b) ? a : b;\n}\n\n/************************************************************** \n * returns the smaller of two integers *\n **************************************************************/ \nlong svd_imin(long a, long b) {\n return (a < b) ? a : b;\n}\n\n/************************************************************** \n * Function scales a vector by a constant. \t\t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nvoid svd_dscal(long n, double da, double *dx, long incx) {\n \n#ifndef USE_BLAS\n cblas_dscal(n,da,dx,incx);\n#else\n long i;\n \n if (n <= 0 || incx == 0) return;\n if (incx < 0) dx += (-n+1) * incx;\n for (i=0; i < n; i++) {\n *dx *= da;\n dx += incx;\n }\n#endif\n \n return;\n}\n\n/************************************************************** \n * function scales a vector by a constant.\t \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nvoid svd_datx(long n, double da, double *dx, long incx, double *dy, long incy) {\n \n#ifdef USE_BLAS\n memset(dy,0,n*sizeof(double));\n cblas_daxpy(n,da,dx,incx,dy,incy);\n#else\n long i;\n if (n <= 0 || incx == 0 || incy == 0 || da == 0.0) return;\n if (incx == 1 && incy == 1) \n for (i=0; i < n; i++) *dy++ = da * (*dx++);\n \n else {\n if (incx < 0) dx += (-n+1) * incx;\n if (incy < 0) dy += (-n+1) * incy;\n for (i=0; i < n; i++) {\n *dy = da * (*dx);\n dx += incx;\n dy += incy;\n }\n }\n#endif\n \n return;\n}\n\n/************************************************************** \n * Function copies a vector x to a vector y\t \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nvoid svd_dcopy(long n, double *dx, long incx, double *dy, long incy) {\n#ifdef USE_BLAS\n cblas_dcopy(n,dx,incx,dy,incy);\n#else\n long i;\n if (n <= 0 || incx == 0 || incy == 0) return;\n if (incx == 1 && incy == 1) \n for (i=0; i < n; i++) *dy++ = *dx++;\n \n else {\n if (incx < 0) dx += (-n+1) * incx;\n if (incy < 0) dy += (-n+1) * incy;\n for (i=0; i < n; i++) {\n *dy = *dx;\n dx += incx;\n dy += incy;\n }\n }\n#endif\n return;\n}\n\n/************************************************************** \n * Function forms the dot product of two vectors. \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \ndouble svd_ddot(long n, double *dx, long incx, double *dy, long incy) {\n\n\n double dot_product;\n#ifdef USE_BLAS\n dot_product=cblas_ddot(n,dx,incx,dy,incy);\n#else\n \n long i;\n \n \n if (n <= 0 || incx == 0 || incy == 0) return(0.0);\n dot_product = 0.0;\n if (incx == 1 && incy == 1) \n for (i=0; i < n; i++) dot_product += (*dx++) * (*dy++);\n else {\n if (incx < 0) dx += (-n+1) * incx;\n if (incy < 0) dy += (-n+1) * incy;\n for (i=0; i < n; i++) {\n dot_product += (*dx) * (*dy);\n dx += incx;\n dy += incy;\n }\n }\n#endif\n return(dot_product);\n}\n\n/************************************************************** \n * Constant times a vector plus a vector \t\t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nvoid svd_daxpy (long n, double da, double *dx, long incx, double *dy, long incy) {\n long i;\n#ifdef USE_BLAS\n cblas_daxpy(n,da,dx,incx,dy,incy);\n#else\n if (n <= 0 || incx == 0 || incy == 0 || da == 0.0) return;\n if (incx == 1 && incy == 1) \n for (i=0; i < n; i++) {\n *dy += da * (*dx++);\n dy++;\n }\n else {\n if (incx < 0) dx += (-n+1) * incx;\n if (incy < 0) dy += (-n+1) * incy;\n for (i=0; i < n; i++) {\n *dy += da * (*dx);\n dx += incx;\n dy += incy;\n }\n }\n#endif\n return;\n}\n\n/********************************************************************* \n * Function sorts array1 and array2 into increasing order for array1 *\n *********************************************************************/\nvoid svd_dsort2(long igap, long n, double *array1, double *array2) {\n\n double temp;\n long i, j, index;\n \n if (!igap) return;\n else {\n for (i = igap; i < n; i++) {\n j = i - igap;\n index = i;\n while (j >= 0 && array1[j] > array1[index]) {\n temp = array1[j];\n array1[j] = array1[index];\n array1[index] = temp;\n temp = array2[j];\n array2[j] = array2[index];\n array2[index] = temp;\n j -= igap;\n index = j + igap;\n }\n } \n }\n svd_dsort2(igap/2,n,array1,array2);\n}\n\n/************************************************************** \n * Function interchanges two vectors\t\t \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nvoid svd_dswap(long n, double *dx, long incx, double *dy, long incy) {\n#ifdef USE_BLAS\n cblas_dswap(n,dx,incx,dy,incy);\n#else\n long i;\n double dtemp;\n \n if (n <= 0 || incx == 0 || incy == 0) return;\n if (incx == 1 && incy == 1) {\n for (i=0; i < n; i++) {\n dtemp = *dy;\n *dy++ = *dx;\n *dx++ = dtemp;\n }\t\n }\n else {\n if (incx < 0) dx += (-n+1) * incx;\n if (incy < 0) dy += (-n+1) * incy;\n for (i=0; i < n; i++) {\n dtemp = *dy;\n *dy = *dx;\n *dx = dtemp;\n dx += incx;\n dy += incy;\n }\n }\n#endif\n}\n\n/***************************************************************** \n * Function finds the index of element having max. absolute value*\n * based on FORTRAN 77 routine from Linpack by J. Dongarra *\n *****************************************************************/ \nlong svd_idamax(long n, double *dx, long incx) {\n long ix,i,imax;\n \n#ifdef USE_BLAS\n imax=cblas_idamax(n,dx,incx);\n#else\n double dtemp, dmax;\n \n if (n < 1) return(-1);\n if (n == 1) return(0);\n if (incx == 0) return(-1);\n \n if (incx < 0) ix = (-n+1) * incx;\n else ix = 0;\n imax = ix;\n dx += ix;\n dmax = fabs(*dx);\n for (i=1; i < n; i++) {\n ix += incx;\n dx += incx;\n dtemp = fabs(*dx);\n if (dtemp > dmax) {\n dmax = dtemp;\n imax = ix;\n }\n }\n#endif\n return(imax);\n}\n\n/**************************************************************\n * multiplication of matrix B by vector x, where B = A'A, *\n * and A is nrow by ncol (nrow >> ncol). Hence, B is of order *\n * n = ncol (y stores product vector).\t\t *\n **************************************************************/\nvoid svd_opb(SMat A, double *x, double *y, double *temp) {\n long i, j, end;\n long *pointr = A->pointr, *rowind = A->rowind;\n double *value = A->value;\n long n = A->cols;\n\n SVDCount[SVD_MXV] += 2;\n memset(y, 0, n * sizeof(double));\n for (i = 0; i < A->rows; i++) temp[i] = 0.0;\n \n for (i = 0; i < A->cols; i++) {\n end = pointr[i+1];\n for (j = pointr[i]; j < end; j++) \n temp[rowind[j]] += value[j] * (*x); \n x++;\n }\n for (i = 0; i < A->cols; i++) {\n end = pointr[i+1];\n for (j = pointr[i]; j < end; j++) \n *y += value[j] * temp[rowind[j]];\n y++;\n }\n return;\n}\n\n/***********************************************************\n * multiplication of matrix A by vector x, where A is \t *\n * nrow by ncol (nrow >> ncol). y stores product vector. *\n ***********************************************************/\nvoid svd_opa(SMat A, double *x, double *y) {\n long end, i, j;\n long *pointr = A->pointr, *rowind = A->rowind;\n double *value = A->value;\n \n SVDCount[SVD_MXV]++;\n memset(y, 0, A->rows * sizeof(double));\n \n for (i = 0; i < A->cols; i++) {\n end = pointr[i+1];\n for (j = pointr[i]; j < end; j++)\n y[rowind[j]] += value[j] * x[i]; \n }\n return;\n}\n\n\n/***********************************************************************\n * *\n *\t\t\t\trandom() *\n * (double precision) *\n ***********************************************************************/\n/***********************************************************************\n\n Description\n -----------\n\n This is a translation of a Fortran-77 uniform random number\n generator. The code is based on theory and suggestions given in\n D. E. Knuth (1969), vol 2. The argument to the function should \n be initialized to an arbitrary integer prior to the first call to \n random. The calling program should not alter the value of the\n argument between subsequent calls to random. Random returns values\n within the interval (0,1).\n\n\n Arguments \n ---------\n\n (input)\n iy\t an integer seed whose value must not be altered by the caller\n\t between subsequent calls\n\n (output)\n random a double precision random number between (0,1)\n\n ***********************************************************************/\ndouble svd_random2(long *iy) {\n static long m2 = 0;\n static long ia, ic, mic;\n static double halfm, s;\n\n /* If first entry, compute (max int) / 2 */\n if (!m2) {\n m2 = 1 << (8 * (int)sizeof(int) - 2); \n halfm = m2;\n\n /* compute multiplier and increment for linear congruential \n * method */\n ia = 8 * (long)(halfm * atan(1.0) / 8.0) + 5;\n ic = 2 * (long)(halfm * (0.5 - sqrt(3.0)/6.0)) + 1;\n mic = (m2-ic) + m2;\n\n /* s is the scale factor for converting to floating point */\n s = 0.5 / halfm;\n }\n\n /* compute next random number */\n *iy = *iy * ia;\n\n /* for computers which do not allow integer overflow on addition */\n if (*iy > mic) *iy = (*iy - m2) - m2;\n\n *iy = *iy + ic;\n\n /* for computers whose word length for addition is greater than\n * for multiplication */\n if (*iy / 2 > m2) *iy = (*iy - m2) - m2;\n \n /* for computers whose integer overflow affects the sign bit */\n if (*iy < 0) *iy = (*iy + m2) + m2;\n\n return((double)(*iy) * s);\n}\n\n/************************************************************** \n *\t\t\t\t\t\t\t *\n * Function finds sqrt(a^2 + b^2) without overflow or *\n * destructive underflow.\t\t\t\t *\n *\t\t\t\t\t\t\t *\n **************************************************************/ \n/************************************************************** \n\n Funtions used\n -------------\n\n UTILITY\tdmax, dmin\n\n **************************************************************/ \ndouble svd_pythag(double a, double b) {\n double p, r, s, t, u, temp;\n\n p = svd_dmax(fabs(a), fabs(b));\n if (p != 0.0) {\n temp = svd_dmin(fabs(a), fabs(b)) / p;\n r = temp * temp; \n t = 4.0 + r;\n while (t != 4.0) {\n\t s = r / t;\n\t u = 1.0 + 2.0 * s;\n\t p *= u;\n\t temp = s / u;\n\t r *= temp * temp;\n\t t = 4.0 + r;\n }\n }\n return(p);\n}\n\n" }, { "path": "GSLAM/svdutil.hpp", "content": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n* Neither the name of the University of Tennessee nor the names of its\n contributors may be used to endorse or promote products derived from this\n software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\nARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\nLIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\nCONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\nSUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\nINTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\nCONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\nARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGE.\n*/\n\n#ifndef SVDUTIL_H\n#define SVDUTIL_H\n\n#include \"svdlib.hpp\"\n#include \n\n#define SAFE_FREE(a) {if (a) {free(a); a = NULL;}}\n#define USE_BLAS\n\n/* Allocates an array of longs. */\nextern long *svd_longArray(long size, char empty, char *name);\n/* Allocates an array of doubles. */\nextern double *svd_doubleArray(long size, char empty, char *name);\n\nextern void svd_debug(char *fmt, ...);\nextern void svd_error(char *fmt, ...);\nextern void svd_fatalError(char *fmt, ...);\nextern FILE *svd_fatalReadFile(char *filename);\nextern FILE *svd_readFile(char *fileName);\nextern FILE *svd_writeFile(char *fileName, char append);\nextern void svd_closeFile(FILE *file);\n\nextern char svd_readBinInt(FILE *file, int *val);\nextern char svd_readBinFloat(FILE *file, float *val);\nextern char svd_writeBinInt(FILE *file, int x);\nextern char svd_writeBinFloat(FILE *file, float r);\n\n/************************************************************** \n * returns |a| if b is positive; else fsign returns -|a| *\n **************************************************************/ \nextern double svd_fsign(double a, double b);\n\n/************************************************************** \n * returns the larger of two double precision numbers *\n **************************************************************/ \nextern double svd_dmax(double a, double b);\n/************************************************************** \n * returns the smaller of two double precision numbers *\n **************************************************************/ \nextern double svd_dmin(double a, double b);\n\n/************************************************************** \n * returns the larger of two integers *\n **************************************************************/ \nextern long svd_imax(long a, long b);\n\n/************************************************************** \n * returns the smaller of two integers *\n **************************************************************/ \nextern long svd_imin(long a, long b);\n\n/************************************************************** \n * Function scales a vector by a constant. \t\t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nextern void svd_dscal(long n, double da, double *dx, long incx);\n//#define svd_dscal cblas_dscal\n/************************************************************** \n * function scales a vector by a constant.\t \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nextern void svd_datx(long n, double da, double *dx, long incx, double *dy, long incy);\n//#define svd_datx cblas_datx\n\n/************************************************************** \n * Function copies a vector x to a vector y\t \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nextern void svd_dcopy(long n, double *dx, long incx, double *dy, long incy);\n\n/************************************************************** \n * Function forms the dot product of two vectors. \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nextern double svd_ddot(long n, double *dx, long incx, double *dy, long incy);\n\n/************************************************************** \n * Constant times a vector plus a vector \t\t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nextern void svd_daxpy (long n, double da, double *dx, long incx, double *dy, long incy);\n\n/********************************************************************* \n * Function sorts array1 and array2 into increasing order for array1 *\n *********************************************************************/\nextern void svd_dsort2(long igap, long n, double *array1, double *array2);\n\n/************************************************************** \n * Function interchanges two vectors\t\t \t *\n * Based on Fortran-77 routine from Linpack by J. Dongarra *\n **************************************************************/ \nextern void svd_dswap(long n, double *dx, long incx, double *dy, long incy);\n\n/***************************************************************** \n * Function finds the index of element having max. absolute value*\n * based on FORTRAN 77 routine from Linpack by J. Dongarra *\n *****************************************************************/ \nextern long svd_idamax(long n, double *dx, long incx);\n\n/**************************************************************\n * multiplication of matrix B by vector x, where B = A'A, *\n * and A is nrow by ncol (nrow >> ncol). Hence, B is of order *\n * n = ncol (y stores product vector).\t\t *\n **************************************************************/\nextern void svd_opb(SMat A, double *x, double *y, double *temp);\n\n/***********************************************************\n * multiplication of matrix A by vector x, where A is \t *\n * nrow by ncol (nrow >> ncol). y stores product vector. *\n ***********************************************************/\nextern void svd_opa(SMat A, double *x, double *y);\n\n/***********************************************************************\n * *\n *\t\t\t\trandom2() *\n * (double precision) *\n ***********************************************************************/\nextern double svd_random2(long *iy);\n\n/************************************************************** \n *\t\t\t\t\t\t\t *\n * Function finds sqrt(a^2 + b^2) without overflow or *\n * destructive underflow.\t\t\t\t *\n *\t\t\t\t\t\t\t *\n **************************************************************/ \nextern double svd_pythag(double a, double b);\n\n\n#endif /* SVDUTIL_H */\n" }, { "path": "GSLAM/time_measurement.hpp", "content": "/******************************************************************************\r\n * Author: Laurent Kneip *\r\n * Contact: kneip.laurent@gmail.com *\r\n * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *\r\n * *\r\n * Redistribution and use in source and binary forms, with or without *\r\n * modification, are permitted provided that the following conditions *\r\n * are met: *\r\n * * Redistributions of source code must retain the above copyright *\r\n * notice, this list of conditions and the following disclaimer. *\r\n * * Redistributions in binary form must reproduce the above copyright *\r\n * notice, this list of conditions and the following disclaimer in the *\r\n * documentation and/or other materials provided with the distribution. *\r\n * * Neither the name of ANU nor the names of its contributors may be *\r\n * used to endorse or promote products derived from this software without *\r\n * specific prior written permission. *\r\n * *\r\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"*\r\n * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *\r\n * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *\r\n * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *\r\n * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *\r\n * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *\r\n * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *\r\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *\r\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *\r\n * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *\r\n * SUCH DAMAGE. *\r\n ******************************************************************************/\r\n\r\n#ifndef OPENGV_TIME_MEASUREMENT_HPP_\r\n#define OPENGV_TIME_MEASUREMENT_HPP_\r\n\r\n#include \r\n#include \r\n\r\n#ifdef WIN32\r\n struct timeval {\r\n int tv_sec;\r\n int tv_usec;\r\n };\r\n\r\n void gettimeofday( timeval * timeofday, int dummy);\r\n#else\r\n #include \r\n#endif\r\n\r\n#define TIMETODOUBLE(x) ( x.tv_sec + x.tv_usec * 1e-6 )\r\n\r\ntimeval\r\ntimeval_minus( const struct timeval &t1, const struct timeval &t2 );\r\n\r\n#endif /* OPENGV_TIME_MEASUREMENT_HPP_ */\r\n" }, { "path": "GSLAM.xcodeproj/project.pbxproj", "content": "// !$*UTF8*$!\n{\n\tarchiveVersion = 1;\n\tclasses = {\n\t};\n\tobjectVersion = 46;\n\tobjects = {\n\n/* Begin PBXBuildFile section */\n\t\t862EB4DC1D9C9906003AABED /* GlobalReconstruction.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 862EB4DB1D9C9906003AABED /* GlobalReconstruction.cpp */; };\n\t\t863568E41D7D32E900F2EDB1 /* ORBextractor.cc in Sources */ = {isa = PBXBuildFile; fileRef = 863568E21D7D32E900F2EDB1 /* ORBextractor.cc */; };\n\t\t863568E71D7D3C9400F2EDB1 /* Accelerate.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 863568E61D7D3C9400F2EDB1 /* Accelerate.framework */; };\n\t\t863568EB1D7D3EB900F2EDB1 /* CoreVideo.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 863568EA1D7D3EB900F2EDB1 /* CoreVideo.framework */; };\n\t\t863568ED1D7D3EC700F2EDB1 /* CoreMedia.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 863568EC1D7D3EC700F2EDB1 /* CoreMedia.framework */; };\n\t\t863568EF1D7D3F0400F2EDB1 /* KeyFrame.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 863568EE1D7D3F0400F2EDB1 /* KeyFrame.cpp */; };\n\t\t863568F21D7D41CE00F2EDB1 /* Frame.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 863568F11D7D41CE00F2EDB1 /* Frame.cpp */; };\n\t\t8645FDD61D9DD5FD0077C95A /* GlobalScaleEstimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8645FDD41D9DD5FD0077C95A /* GlobalScaleEstimation.cpp */; };\n\t\t864C60C81D80015300F87039 /* ORBmatcher.cc in Sources */ = {isa = PBXBuildFile; fileRef = 864C60C61D80015300F87039 /* ORBmatcher.cc */; };\n\t\t8668D70E1D7E381500FC7DE5 /* Transform.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8668D70D1D7E381500FC7DE5 /* Transform.cpp */; };\n\t\t867170801D85164F00A8AE13 /* TrackUtil.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8671707F1D85164F00A8AE13 /* TrackUtil.cpp */; };\n\t\t867170821D851E2200A8AE13 /* Estimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 867170811D851E2200A8AE13 /* Estimation.cpp */; };\n\t\t86737C1C1D7BEBA8005AAEAC /* Foundation.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86737C1B1D7BEBA8005AAEAC /* Foundation.framework */; };\n\t\t86737C1E1D7BEBB4005AAEAC /* CoreFoundation.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86737C1D1D7BEBB4005AAEAC /* CoreFoundation.framework */; };\n\t\t86737C211D7BF63F005AAEAC /* System.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86737C201D7BF63F005AAEAC /* System.cpp */; };\n\t\t86737C291D7C0513005AAEAC /* KeyFrameDatabase.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86737C271D7C0513005AAEAC /* KeyFrameDatabase.cpp */; };\n\t\t8674FBB41D76587800FB83D1 /* main.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 8674FBB31D76587800FB83D1 /* main.cpp */; };\n\t\t867F57501D823A5200CF0965 /* KLTUtil.h in Sources */ = {isa = PBXBuildFile; fileRef = 867F574F1D823A5200CF0965 /* KLTUtil.h */; };\n\t\t868033AE1D81497B003622CA /* convolve.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 868033AD1D81497B003622CA /* convolve.cpp */; };\n\t\t868033B01D814A73003622CA /* KLTUtil.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 868033AF1D814A73003622CA /* KLTUtil.cpp */; };\n\t\t86A145A31DA3293000340813 /* GlobalTranslationEstimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86A145A21DA3293000340813 /* GlobalTranslationEstimation.cpp */; };\n\t\t86A1E4341D9206A1003EB022 /* MapPoint.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86A1E4331D9206A1003EB022 /* MapPoint.cpp */; };\n\t\t86A7B3F31D81EBEC004C0F28 /* pyramid.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86A7B3F11D81EBEC004C0F28 /* pyramid.cpp */; };\n\t\t86A8C9011DE0275C008F117C /* AVFoundation.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A8C9001DE0275C008F117C /* AVFoundation.framework */; };\n\t\t86A8C9031DE061F4008F117C /* OpenGL.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = 86A8C9021DE061F4008F117C /* OpenGL.framework */; };\n\t\t86B4CDBA1DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86B4CDB91DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp */; };\n\t\t86B832671D97A49200A0B460 /* KeyFrameConnection.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86B832651D97A49200A0B460 /* KeyFrameConnection.cpp */; };\n\t\t86BA29661D86A793003EBBDE /* las2.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86BA29611D86A793003EBBDE /* las2.cpp */; };\n\t\t86BA29671D86A793003EBBDE /* svdlib.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86BA29621D86A793003EBBDE /* svdlib.cpp */; };\n\t\t86BA29681D86A793003EBBDE /* svdutil.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86BA29641D86A793003EBBDE /* svdutil.cpp */; };\n\t\t86E4B7B21D8E42F200058234 /* Geometry.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86E4B7B11D8E42F200058234 /* Geometry.cpp */; };\n\t\t86F47B4C1D80E2E600F47ACF /* error.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 86F47B451D80E2E600F47ACF /* error.cpp */; };\n\t\tC4191123209344AF000BE72F /* libtheia.a in Frameworks */ = {isa = PBXBuildFile; fileRef = C4191122209344AF000BE72F /* libtheia.a */; };\n/* End PBXBuildFile section */\n\n/* Begin PBXCopyFilesBuildPhase section */\n\t\t8674FBAE1D76587800FB83D1 /* CopyFiles */ = {\n\t\t\tisa = PBXCopyFilesBuildPhase;\n\t\t\tbuildActionMask = 2147483647;\n\t\t\tdstPath = /usr/share/man/man1/;\n\t\t\tdstSubfolderSpec = 0;\n\t\t\tfiles = (\n\t\t\t);\n\t\t\trunOnlyForDeploymentPostprocessing = 1;\n\t\t};\n/* End PBXCopyFilesBuildPhase section */\n\n/* Begin PBXFileReference section */\n\t\t862EB4DB1D9C9906003AABED /* GlobalReconstruction.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalReconstruction.cpp; sourceTree = \"\"; };\n\t\t863568E21D7D32E900F2EDB1 /* ORBextractor.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = ORBextractor.cc; sourceTree = \"\"; };\n\t\t863568E31D7D32E900F2EDB1 /* ORBextractor.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ORBextractor.h; sourceTree = \"\"; };\n\t\t863568E51D7D37E200F2EDB1 /* MapPoint.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = MapPoint.h; sourceTree = \"\"; };\n\t\t863568E61D7D3C9400F2EDB1 /* Accelerate.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = Accelerate.framework; path = System/Library/Frameworks/Accelerate.framework; sourceTree = SDKROOT; };\n\t\t863568EA1D7D3EB900F2EDB1 /* CoreVideo.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreVideo.framework; path = System/Library/Frameworks/CoreVideo.framework; sourceTree = SDKROOT; };\n\t\t863568EC1D7D3EC700F2EDB1 /* CoreMedia.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreMedia.framework; path = System/Library/Frameworks/CoreMedia.framework; sourceTree = SDKROOT; };\n\t\t863568EE1D7D3F0400F2EDB1 /* KeyFrame.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KeyFrame.cpp; sourceTree = \"\"; };\n\t\t863568F11D7D41CE00F2EDB1 /* Frame.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Frame.cpp; sourceTree = \"\"; };\n\t\t863568F31D7D425F00F2EDB1 /* Settings.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Settings.h; sourceTree = \"\"; };\n\t\t863568F41D7D436500F2EDB1 /* Estimation.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Estimation.h; sourceTree = \"\"; };\n\t\t863568F61D7D5E0B00F2EDB1 /* Transform.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Transform.h; sourceTree = \"\"; };\n\t\t8645FDD41D9DD5FD0077C95A /* GlobalScaleEstimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalScaleEstimation.cpp; sourceTree = \"\"; };\n\t\t864C60C61D80015300F87039 /* ORBmatcher.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = ORBmatcher.cc; sourceTree = \"\"; };\n\t\t864C60C71D80015300F87039 /* ORBmatcher.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ORBmatcher.h; sourceTree = \"\"; };\n\t\t865F271D1D88D8F9002100FD /* selectGoodFeatures.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = selectGoodFeatures.hpp; sourceTree = \"\"; };\n\t\t8668D70D1D7E381500FC7DE5 /* Transform.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Transform.cpp; sourceTree = \"\"; };\n\t\t8671707E1D85163300A8AE13 /* TrackUtil.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = TrackUtil.h; sourceTree = \"\"; };\n\t\t8671707F1D85164F00A8AE13 /* TrackUtil.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = TrackUtil.cpp; sourceTree = \"\"; };\n\t\t867170811D851E2200A8AE13 /* Estimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Estimation.cpp; sourceTree = \"\"; };\n\t\t86737C1B1D7BEBA8005AAEAC /* Foundation.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = Foundation.framework; path = System/Library/Frameworks/Foundation.framework; sourceTree = SDKROOT; };\n\t\t86737C1D1D7BEBB4005AAEAC /* CoreFoundation.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreFoundation.framework; path = System/Library/Frameworks/CoreFoundation.framework; sourceTree = SDKROOT; };\n\t\t86737C1F1D7BF625005AAEAC /* System.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = System.h; sourceTree = \"\"; };\n\t\t86737C201D7BF63F005AAEAC /* System.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = System.cpp; sourceTree = \"\"; };\n\t\t86737C221D7BFDB4005AAEAC /* ORBVocabulary.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ORBVocabulary.h; sourceTree = \"\"; };\n\t\t86737C261D7C043D005AAEAC /* DBoW2 */ = {isa = PBXFileReference; lastKnownFileType = folder; path = DBoW2; sourceTree = \"\"; };\n\t\t86737C271D7C0513005AAEAC /* KeyFrameDatabase.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KeyFrameDatabase.cpp; sourceTree = \"\"; };\n\t\t86737C281D7C0513005AAEAC /* KeyFrameDatabase.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KeyFrameDatabase.h; sourceTree = \"\"; };\n\t\t86737C2A1D7C05E1005AAEAC /* Frame.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = Frame.h; sourceTree = \"\"; };\n\t\t86737C2B1D7C05E1005AAEAC /* KeyFrame.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KeyFrame.h; sourceTree = \"\"; };\n\t\t8674FBB01D76587800FB83D1 /* GSLAM */ = {isa = PBXFileReference; explicitFileType = \"compiled.mach-o.executable\"; includeInIndex = 0; path = GSLAM; sourceTree = BUILT_PRODUCTS_DIR; };\n\t\t8674FBB31D76587800FB83D1 /* main.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = main.cpp; sourceTree = \"\"; };\n\t\t867F574F1D823A5200CF0965 /* KLTUtil.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KLTUtil.h; sourceTree = \"\"; };\n\t\t868033AC1D8148A0003622CA /* convolve.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = convolve.h; sourceTree = \"\"; };\n\t\t868033AD1D81497B003622CA /* convolve.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = convolve.cpp; sourceTree = \"\"; };\n\t\t868033AF1D814A73003622CA /* KLTUtil.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KLTUtil.cpp; sourceTree = \"\"; };\n\t\t86A145A21DA3293000340813 /* GlobalTranslationEstimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalTranslationEstimation.cpp; sourceTree = \"\"; };\n\t\t86A1E4331D9206A1003EB022 /* MapPoint.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = MapPoint.cpp; sourceTree = \"\"; };\n\t\t86A7B3EF1D81D9E5004C0F28 /* GlobalReconstruction.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = GlobalReconstruction.h; sourceTree = \"\"; };\n\t\t86A7B3F01D81E591004C0F28 /* pyramid.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = pyramid.h; sourceTree = \"\"; };\n\t\t86A7B3F11D81EBEC004C0F28 /* pyramid.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = pyramid.cpp; sourceTree = \"\"; };\n\t\t86A8C9001DE0275C008F117C /* AVFoundation.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = AVFoundation.framework; path = System/Library/Frameworks/AVFoundation.framework; sourceTree = SDKROOT; };\n\t\t86A8C9021DE061F4008F117C /* OpenGL.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = OpenGL.framework; path = System/Library/Frameworks/OpenGL.framework; sourceTree = SDKROOT; };\n\t\t86A8C9041DE0646F008F117C /* Drawer.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Drawer.h; sourceTree = \"\"; };\n\t\t86A8C9061DE06768008F117C /* Drawer.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = Drawer.cpp; sourceTree = \"\"; };\n\t\t86B4CDB91DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = GlobalRotationEstimation.cpp; sourceTree = \"\"; };\n\t\t86B832651D97A49200A0B460 /* KeyFrameConnection.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = KeyFrameConnection.cpp; sourceTree = \"\"; };\n\t\t86B832661D97A49200A0B460 /* KeyFrameConnection.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KeyFrameConnection.h; sourceTree = \"\"; };\n\t\t86BA29611D86A793003EBBDE /* las2.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = las2.cpp; sourceTree = \"\"; };\n\t\t86BA29621D86A793003EBBDE /* svdlib.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = svdlib.cpp; sourceTree = \"\"; };\n\t\t86BA29631D86A793003EBBDE /* svdlib.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = svdlib.hpp; sourceTree = \"\"; };\n\t\t86BA29641D86A793003EBBDE /* svdutil.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = svdutil.cpp; sourceTree = \"\"; };\n\t\t86BA29651D86A793003EBBDE /* svdutil.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = svdutil.hpp; sourceTree = \"\"; };\n\t\t86BA296B1D86A867003EBBDE /* ImageGrids.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = ImageGrids.hpp; sourceTree = \"\"; };\n\t\t86BA296C1D86A867003EBBDE /* IMU.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = IMU.hpp; sourceTree = \"\"; };\n\t\t86E4B7B01D8E3D4300058234 /* Geometry.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = Geometry.h; sourceTree = \"\"; };\n\t\t86E4B7B11D8E42F200058234 /* Geometry.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = Geometry.cpp; sourceTree = \"\"; };\n\t\t86EA059D1DA1B5FD00A32616 /* L1Solver.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = L1Solver.h; sourceTree = \"\"; };\n\t\t86F47B451D80E2E600F47ACF /* error.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = error.cpp; sourceTree = \"\"; };\n\t\t86F47B461D80E2E600F47ACF /* error.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = error.h; sourceTree = \"\"; };\n\t\t86F47B481D80E2E600F47ACF /* KLT.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = KLT.h; sourceTree = \"\"; };\n\t\t86F47B4B1D80E2E600F47ACF /* LK.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = LK.hpp; sourceTree = \"\"; };\n\t\tC4191122209344AF000BE72F /* libtheia.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libtheia.a; path = GSLAM/lib/libtheia.a; sourceTree = \"\"; };\n\t\tC419112420934AD8000BE72F /* DUtils */ = {isa = PBXFileReference; lastKnownFileType = folder; path = DUtils; sourceTree = \"\"; };\n\t\tC4B124381E51504300670AFD /* Homography.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = Homography.hpp; sourceTree = \"\"; };\n\t\tC4B124391E57C44B00670AFD /* RelativeMotion.hpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.h; path = RelativeMotion.hpp; sourceTree = \"\"; };\n/* End PBXFileReference section */\n\n/* Begin PBXFrameworksBuildPhase section */\n\t\t8674FBAD1D76587800FB83D1 /* Frameworks */ = {\n\t\t\tisa = PBXFrameworksBuildPhase;\n\t\t\tbuildActionMask = 2147483647;\n\t\t\tfiles = (\n\t\t\t\t86A8C9031DE061F4008F117C /* OpenGL.framework in Frameworks */,\n\t\t\t\t86A8C9011DE0275C008F117C /* AVFoundation.framework in Frameworks */,\n\t\t\t\t863568ED1D7D3EC700F2EDB1 /* CoreMedia.framework in Frameworks */,\n\t\t\t\tC4191123209344AF000BE72F /* libtheia.a in Frameworks */,\n\t\t\t\t863568EB1D7D3EB900F2EDB1 /* CoreVideo.framework in Frameworks */,\n\t\t\t\t863568E71D7D3C9400F2EDB1 /* Accelerate.framework in Frameworks */,\n\t\t\t\t86737C1E1D7BEBB4005AAEAC /* CoreFoundation.framework in Frameworks */,\n\t\t\t\t86737C1C1D7BEBA8005AAEAC /* Foundation.framework in Frameworks */,\n\t\t\t);\n\t\t\trunOnlyForDeploymentPostprocessing = 0;\n\t\t};\n/* End PBXFrameworksBuildPhase section */\n\n/* Begin PBXGroup section */\n\t\t863568E81D7D3D4900F2EDB1 /* ORB */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t864C60C61D80015300F87039 /* ORBmatcher.cc */,\n\t\t\t\t864C60C71D80015300F87039 /* ORBmatcher.h */,\n\t\t\t\t863568E21D7D32E900F2EDB1 /* ORBextractor.cc */,\n\t\t\t\t863568E31D7D32E900F2EDB1 /* ORBextractor.h */,\n\t\t\t\t86737C221D7BFDB4005AAEAC /* ORBVocabulary.h */,\n\t\t\t);\n\t\t\tname = ORB;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t863568E91D7D3DB400F2EDB1 /* Frame */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t86737C2A1D7C05E1005AAEAC /* Frame.h */,\n\t\t\t\t863568F11D7D41CE00F2EDB1 /* Frame.cpp */,\n\t\t\t\t86737C2B1D7C05E1005AAEAC /* KeyFrame.h */,\n\t\t\t\t863568EE1D7D3F0400F2EDB1 /* KeyFrame.cpp */,\n\t\t\t\t86B832651D97A49200A0B460 /* KeyFrameConnection.cpp */,\n\t\t\t\t86B832661D97A49200A0B460 /* KeyFrameConnection.h */,\n\t\t\t\t86737C281D7C0513005AAEAC /* KeyFrameDatabase.h */,\n\t\t\t\t86737C271D7C0513005AAEAC /* KeyFrameDatabase.cpp */,\n\t\t\t);\n\t\t\tname = Frame;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t863568F01D7D3F2700F2EDB1 /* Geometry */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t863568E51D7D37E200F2EDB1 /* MapPoint.h */,\n\t\t\t\t86A1E4331D9206A1003EB022 /* MapPoint.cpp */,\n\t\t\t\t867170811D851E2200A8AE13 /* Estimation.cpp */,\n\t\t\t\t863568F41D7D436500F2EDB1 /* Estimation.h */,\n\t\t\t\t863568F61D7D5E0B00F2EDB1 /* Transform.h */,\n\t\t\t\t8668D70D1D7E381500FC7DE5 /* Transform.cpp */,\n\t\t\t\t86EA059D1DA1B5FD00A32616 /* L1Solver.h */,\n\t\t\t\t86A7B3EF1D81D9E5004C0F28 /* GlobalReconstruction.h */,\n\t\t\t\t862EB4DB1D9C9906003AABED /* GlobalReconstruction.cpp */,\n\t\t\t\t86A145A21DA3293000340813 /* GlobalTranslationEstimation.cpp */,\n\t\t\t\t86B4CDB91DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp */,\n\t\t\t\t8645FDD41D9DD5FD0077C95A /* GlobalScaleEstimation.cpp */,\n\t\t\t\t86E4B7B01D8E3D4300058234 /* Geometry.h */,\n\t\t\t\t86E4B7B11D8E42F200058234 /* Geometry.cpp */,\n\t\t\t\tC4B124391E57C44B00670AFD /* RelativeMotion.hpp */,\n\t\t\t);\n\t\t\tname = Geometry;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t863568F51D7D562800F2EDB1 /* Tracking */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\tC4B124381E51504300670AFD /* Homography.hpp */,\n\t\t\t\t865F271D1D88D8F9002100FD /* selectGoodFeatures.hpp */,\n\t\t\t\t868033AC1D8148A0003622CA /* convolve.h */,\n\t\t\t\t868033AD1D81497B003622CA /* convolve.cpp */,\n\t\t\t\t86F47B451D80E2E600F47ACF /* error.cpp */,\n\t\t\t\t86F47B461D80E2E600F47ACF /* error.h */,\n\t\t\t\t86F47B481D80E2E600F47ACF /* KLT.h */,\n\t\t\t\t867F574F1D823A5200CF0965 /* KLTUtil.h */,\n\t\t\t\t868033AF1D814A73003622CA /* KLTUtil.cpp */,\n\t\t\t\t86A7B3F01D81E591004C0F28 /* pyramid.h */,\n\t\t\t\t86A7B3F11D81EBEC004C0F28 /* pyramid.cpp */,\n\t\t\t\t86F47B4B1D80E2E600F47ACF /* LK.hpp */,\n\t\t\t\t8671707E1D85163300A8AE13 /* TrackUtil.h */,\n\t\t\t\t8671707F1D85164F00A8AE13 /* TrackUtil.cpp */,\n\t\t\t);\n\t\t\tname = Tracking;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t8674FBA71D76587800FB83D1 = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\tC4191122209344AF000BE72F /* libtheia.a */,\n\t\t\t\t86A8C9021DE061F4008F117C /* OpenGL.framework */,\n\t\t\t\t86A8C9001DE0275C008F117C /* AVFoundation.framework */,\n\t\t\t\t863568EC1D7D3EC700F2EDB1 /* CoreMedia.framework */,\n\t\t\t\t863568EA1D7D3EB900F2EDB1 /* CoreVideo.framework */,\n\t\t\t\t863568E61D7D3C9400F2EDB1 /* Accelerate.framework */,\n\t\t\t\t86737C1D1D7BEBB4005AAEAC /* CoreFoundation.framework */,\n\t\t\t\t86737C1B1D7BEBA8005AAEAC /* Foundation.framework */,\n\t\t\t\t8674FBB21D76587800FB83D1 /* GSLAM */,\n\t\t\t\t8674FBB11D76587800FB83D1 /* Products */,\n\t\t\t);\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t8674FBB11D76587800FB83D1 /* Products */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t8674FBB01D76587800FB83D1 /* GSLAM */,\n\t\t\t);\n\t\t\tname = Products;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t8674FBB21D76587800FB83D1 /* GSLAM */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t86A8C9051DE06478008F117C /* GUI */,\n\t\t\t\t86BA296A1D86A82F003EBBDE /* IMU */,\n\t\t\t\t86BA29691D86A79E003EBBDE /* svd */,\n\t\t\t\t863568F51D7D562800F2EDB1 /* Tracking */,\n\t\t\t\t863568F01D7D3F2700F2EDB1 /* Geometry */,\n\t\t\t\t863568E91D7D3DB400F2EDB1 /* Frame */,\n\t\t\t\t863568E81D7D3D4900F2EDB1 /* ORB */,\n\t\t\t\tC419112420934AD8000BE72F /* DUtils */,\n\t\t\t\t86737C261D7C043D005AAEAC /* DBoW2 */,\n\t\t\t\t863568F31D7D425F00F2EDB1 /* Settings.h */,\n\t\t\t\t86737C1F1D7BF625005AAEAC /* System.h */,\n\t\t\t\t86737C201D7BF63F005AAEAC /* System.cpp */,\n\t\t\t\t8674FBB31D76587800FB83D1 /* main.cpp */,\n\t\t\t);\n\t\t\tpath = GSLAM;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t86A8C9051DE06478008F117C /* GUI */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t86A8C9041DE0646F008F117C /* Drawer.h */,\n\t\t\t\t86A8C9061DE06768008F117C /* Drawer.cpp */,\n\t\t\t);\n\t\t\tname = GUI;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t86BA29691D86A79E003EBBDE /* svd */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t86BA29611D86A793003EBBDE /* las2.cpp */,\n\t\t\t\t86BA29621D86A793003EBBDE /* svdlib.cpp */,\n\t\t\t\t86BA29631D86A793003EBBDE /* svdlib.hpp */,\n\t\t\t\t86BA29641D86A793003EBBDE /* svdutil.cpp */,\n\t\t\t\t86BA29651D86A793003EBBDE /* svdutil.hpp */,\n\t\t\t);\n\t\t\tname = svd;\n\t\t\tsourceTree = \"\";\n\t\t};\n\t\t86BA296A1D86A82F003EBBDE /* IMU */ = {\n\t\t\tisa = PBXGroup;\n\t\t\tchildren = (\n\t\t\t\t86BA296B1D86A867003EBBDE /* ImageGrids.hpp */,\n\t\t\t\t86BA296C1D86A867003EBBDE /* IMU.hpp */,\n\t\t\t);\n\t\t\tname = IMU;\n\t\t\tsourceTree = \"\";\n\t\t};\n/* End PBXGroup section */\n\n/* Begin PBXNativeTarget section */\n\t\t8674FBAF1D76587800FB83D1 /* GSLAM */ = {\n\t\t\tisa = PBXNativeTarget;\n\t\t\tbuildConfigurationList = 8674FBB71D76587800FB83D1 /* Build configuration list for PBXNativeTarget \"GSLAM\" */;\n\t\t\tbuildPhases = (\n\t\t\t\t8674FBAC1D76587800FB83D1 /* Sources */,\n\t\t\t\t8674FBAD1D76587800FB83D1 /* Frameworks */,\n\t\t\t\t8674FBAE1D76587800FB83D1 /* CopyFiles */,\n\t\t\t);\n\t\t\tbuildRules = (\n\t\t\t);\n\t\t\tdependencies = (\n\t\t\t);\n\t\t\tname = GSLAM;\n\t\t\tproductName = GSLAM;\n\t\t\tproductReference = 8674FBB01D76587800FB83D1 /* GSLAM */;\n\t\t\tproductType = \"com.apple.product-type.tool\";\n\t\t};\n/* End PBXNativeTarget section */\n\n/* Begin PBXProject section */\n\t\t8674FBA81D76587800FB83D1 /* Project object */ = {\n\t\t\tisa = PBXProject;\n\t\t\tattributes = {\n\t\t\t\tLastUpgradeCheck = 0730;\n\t\t\t\tORGANIZATIONNAME = ctang;\n\t\t\t\tTargetAttributes = {\n\t\t\t\t\t8674FBAF1D76587800FB83D1 = {\n\t\t\t\t\t\tCreatedOnToolsVersion = 7.3.1;\n\t\t\t\t\t};\n\t\t\t\t};\n\t\t\t};\n\t\t\tbuildConfigurationList = 8674FBAB1D76587800FB83D1 /* Build configuration list for PBXProject \"GSLAM\" */;\n\t\t\tcompatibilityVersion = \"Xcode 3.2\";\n\t\t\tdevelopmentRegion = English;\n\t\t\thasScannedForEncodings = 0;\n\t\t\tknownRegions = (\n\t\t\t\ten,\n\t\t\t);\n\t\t\tmainGroup = 8674FBA71D76587800FB83D1;\n\t\t\tproductRefGroup = 8674FBB11D76587800FB83D1 /* Products */;\n\t\t\tprojectDirPath = \"\";\n\t\t\tprojectRoot = \"\";\n\t\t\ttargets = (\n\t\t\t\t8674FBAF1D76587800FB83D1 /* GSLAM */,\n\t\t\t);\n\t\t};\n/* End PBXProject section */\n\n/* Begin PBXSourcesBuildPhase section */\n\t\t8674FBAC1D76587800FB83D1 /* Sources */ = {\n\t\t\tisa = PBXSourcesBuildPhase;\n\t\t\tbuildActionMask = 2147483647;\n\t\t\tfiles = (\n\t\t\t\t86A7B3F31D81EBEC004C0F28 /* pyramid.cpp in Sources */,\n\t\t\t\t86BA29681D86A793003EBBDE /* svdutil.cpp in Sources */,\n\t\t\t\t868033AE1D81497B003622CA /* convolve.cpp in Sources */,\n\t\t\t\t8668D70E1D7E381500FC7DE5 /* Transform.cpp in Sources */,\n\t\t\t\t8645FDD61D9DD5FD0077C95A /* GlobalScaleEstimation.cpp in Sources */,\n\t\t\t\t86737C211D7BF63F005AAEAC /* System.cpp in Sources */,\n\t\t\t\t86B832671D97A49200A0B460 /* KeyFrameConnection.cpp in Sources */,\n\t\t\t\t86A1E4341D9206A1003EB022 /* MapPoint.cpp in Sources */,\n\t\t\t\t868033B01D814A73003622CA /* KLTUtil.cpp in Sources */,\n\t\t\t\t86737C291D7C0513005AAEAC /* KeyFrameDatabase.cpp in Sources */,\n\t\t\t\t86F47B4C1D80E2E600F47ACF /* error.cpp in Sources */,\n\t\t\t\t86B4CDBA1DA0EE9100BA3A04 /* GlobalRotationEstimation.cpp in Sources */,\n\t\t\t\t863568EF1D7D3F0400F2EDB1 /* KeyFrame.cpp in Sources */,\n\t\t\t\t867170801D85164F00A8AE13 /* TrackUtil.cpp in Sources */,\n\t\t\t\t86BA29671D86A793003EBBDE /* svdlib.cpp in Sources */,\n\t\t\t\t864C60C81D80015300F87039 /* ORBmatcher.cc in Sources */,\n\t\t\t\t863568F21D7D41CE00F2EDB1 /* Frame.cpp in Sources */,\n\t\t\t\t86BA29661D86A793003EBBDE /* las2.cpp in Sources */,\n\t\t\t\t86E4B7B21D8E42F200058234 /* Geometry.cpp in Sources */,\n\t\t\t\t867170821D851E2200A8AE13 /* Estimation.cpp in Sources */,\n\t\t\t\t867F57501D823A5200CF0965 /* KLTUtil.h in Sources */,\n\t\t\t\t863568E41D7D32E900F2EDB1 /* ORBextractor.cc in Sources */,\n\t\t\t\t86A145A31DA3293000340813 /* GlobalTranslationEstimation.cpp in Sources */,\n\t\t\t\t862EB4DC1D9C9906003AABED /* GlobalReconstruction.cpp in Sources */,\n\t\t\t\t8674FBB41D76587800FB83D1 /* main.cpp in Sources */,\n\t\t\t);\n\t\t\trunOnlyForDeploymentPostprocessing = 0;\n\t\t};\n/* End PBXSourcesBuildPhase section */\n\n/* Begin XCBuildConfiguration section */\n\t\t8674FBB51D76587800FB83D1 /* Debug */ = {\n\t\t\tisa = XCBuildConfiguration;\n\t\t\tbuildSettings = {\n\t\t\t\tALWAYS_SEARCH_USER_PATHS = NO;\n\t\t\t\tCLANG_ANALYZER_NONNULL = YES;\n\t\t\t\tCLANG_CXX_LANGUAGE_STANDARD = \"gnu++0x\";\n\t\t\t\tCLANG_CXX_LIBRARY = \"libc++\";\n\t\t\t\tCLANG_ENABLE_MODULES = YES;\n\t\t\t\tCLANG_ENABLE_OBJC_ARC = YES;\n\t\t\t\tCLANG_WARN_BOOL_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_CONSTANT_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;\n\t\t\t\tCLANG_WARN_EMPTY_BODY = YES;\n\t\t\t\tCLANG_WARN_ENUM_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_INT_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;\n\t\t\t\tCLANG_WARN_UNREACHABLE_CODE = YES;\n\t\t\t\tCLANG_WARN__DUPLICATE_METHOD_MATCH = YES;\n\t\t\t\tCODE_SIGN_IDENTITY = \"-\";\n\t\t\t\tCOPY_PHASE_STRIP = NO;\n\t\t\t\tDEBUG_INFORMATION_FORMAT = dwarf;\n\t\t\t\tENABLE_STRICT_OBJC_MSGSEND = YES;\n\t\t\t\tENABLE_TESTABILITY = YES;\n\t\t\t\tGCC_C_LANGUAGE_STANDARD = gnu99;\n\t\t\t\tGCC_DYNAMIC_NO_PIC = NO;\n\t\t\t\tGCC_NO_COMMON_BLOCKS = YES;\n\t\t\t\tGCC_OPTIMIZATION_LEVEL = 0;\n\t\t\t\tGCC_PREPROCESSOR_DEFINITIONS = (\n\t\t\t\t\t\"DEBUG=1\",\n\t\t\t\t\t\"$(inherited)\",\n\t\t\t\t);\n\t\t\t\tGCC_WARN_64_TO_32_BIT_CONVERSION = YES;\n\t\t\t\tGCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;\n\t\t\t\tGCC_WARN_UNDECLARED_SELECTOR = YES;\n\t\t\t\tGCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE;\n\t\t\t\tGCC_WARN_UNUSED_FUNCTION = YES;\n\t\t\t\tGCC_WARN_UNUSED_VARIABLE = YES;\n\t\t\t\tMACOSX_DEPLOYMENT_TARGET = 10.11;\n\t\t\t\tMTL_ENABLE_DEBUG_INFO = YES;\n\t\t\t\tONLY_ACTIVE_ARCH = YES;\n\t\t\t\tSDKROOT = macosx;\n\t\t\t};\n\t\t\tname = Debug;\n\t\t};\n\t\t8674FBB61D76587800FB83D1 /* Release */ = {\n\t\t\tisa = XCBuildConfiguration;\n\t\t\tbuildSettings = {\n\t\t\t\tALWAYS_SEARCH_USER_PATHS = NO;\n\t\t\t\tCLANG_ANALYZER_NONNULL = YES;\n\t\t\t\tCLANG_CXX_LANGUAGE_STANDARD = \"gnu++0x\";\n\t\t\t\tCLANG_CXX_LIBRARY = \"libc++\";\n\t\t\t\tCLANG_ENABLE_MODULES = YES;\n\t\t\t\tCLANG_ENABLE_OBJC_ARC = YES;\n\t\t\t\tCLANG_WARN_BOOL_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_CONSTANT_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;\n\t\t\t\tCLANG_WARN_EMPTY_BODY = YES;\n\t\t\t\tCLANG_WARN_ENUM_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_INT_CONVERSION = YES;\n\t\t\t\tCLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;\n\t\t\t\tCLANG_WARN_UNREACHABLE_CODE = YES;\n\t\t\t\tCLANG_WARN__DUPLICATE_METHOD_MATCH = YES;\n\t\t\t\tCODE_SIGN_IDENTITY = \"-\";\n\t\t\t\tCOPY_PHASE_STRIP = NO;\n\t\t\t\tDEBUG_INFORMATION_FORMAT = \"dwarf-with-dsym\";\n\t\t\t\tENABLE_NS_ASSERTIONS = NO;\n\t\t\t\tENABLE_STRICT_OBJC_MSGSEND = YES;\n\t\t\t\tGCC_C_LANGUAGE_STANDARD = gnu99;\n\t\t\t\tGCC_NO_COMMON_BLOCKS = YES;\n\t\t\t\tGCC_WARN_64_TO_32_BIT_CONVERSION = YES;\n\t\t\t\tGCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;\n\t\t\t\tGCC_WARN_UNDECLARED_SELECTOR = YES;\n\t\t\t\tGCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE;\n\t\t\t\tGCC_WARN_UNUSED_FUNCTION = YES;\n\t\t\t\tGCC_WARN_UNUSED_VARIABLE = YES;\n\t\t\t\tMACOSX_DEPLOYMENT_TARGET = 10.11;\n\t\t\t\tMTL_ENABLE_DEBUG_INFO = NO;\n\t\t\t\tSDKROOT = macosx;\n\t\t\t};\n\t\t\tname = Release;\n\t\t};\n\t\t8674FBB81D76587800FB83D1 /* Debug */ = {\n\t\t\tisa = XCBuildConfiguration;\n\t\t\tbuildSettings = {\n\t\t\t\tARCHS = \"$(ARCHS_STANDARD_64_BIT)\";\n\t\t\t\tCLANG_CXX_LANGUAGE_STANDARD = \"c++0x\";\n\t\t\t\tCLANG_CXX_LIBRARY = \"libc++\";\n\t\t\t\tCLANG_ENABLE_OBJC_ARC = YES;\n\t\t\t\tCLANG_X86_VECTOR_INSTRUCTIONS = avx;\n\t\t\t\tGCC_C_LANGUAGE_STANDARD = c11;\n\t\t\t\tGCC_INPUT_FILETYPE = sourcecode.cpp.objcpp;\n\t\t\t\t\"HEADER_SEARCH_PATHS[arch=*]\" = (\n\t\t\t\t\t/usr/local/include,\n\t\t\t\t\t/usr/local/include/eigen3,\n\t\t\t\t);\n\t\t\t\tLD_RUNPATH_SEARCH_PATHS = \"\";\n\t\t\t\tLIBRARY_SEARCH_PATHS = (\n\t\t\t\t\t\"$(inherited)\",\n\t\t\t\t\t\"$(PROJECT_DIR)/GSLAM/lib\",\n\t\t\t\t\t/usr/local/lib,\n\t\t\t\t\t\"$(PROJECT_DIR)\",\n\t\t\t\t\t\"$(PROJECT_DIR)/GSLAM\",\n\t\t\t\t);\n\t\t\t\tOTHER_LDFLAGS = \"\";\n\t\t\t\tPRODUCT_NAME = \"$(TARGET_NAME)\";\n\t\t\t\tSDKROOT = macosx10.13;\n\t\t\t};\n\t\t\tname = Debug;\n\t\t};\n\t\t8674FBB91D76587800FB83D1 /* Release */ = {\n\t\t\tisa = XCBuildConfiguration;\n\t\t\tbuildSettings = {\n\t\t\t\tARCHS = \"$(ARCHS_STANDARD_64_BIT)\";\n\t\t\t\tCLANG_CXX_LANGUAGE_STANDARD = \"c++0x\";\n\t\t\t\tCLANG_CXX_LIBRARY = \"libc++\";\n\t\t\t\tCLANG_ENABLE_OBJC_ARC = YES;\n\t\t\t\tCLANG_X86_VECTOR_INSTRUCTIONS = avx;\n\t\t\t\tENABLE_NS_ASSERTIONS = YES;\n\t\t\t\tGCC_C_LANGUAGE_STANDARD = c11;\n\t\t\t\tGCC_INPUT_FILETYPE = sourcecode.cpp.objcpp;\n\t\t\t\tGCC_OPTIMIZATION_LEVEL = 3;\n\t\t\t\t\"HEADER_SEARCH_PATHS[arch=*]\" = (\n\t\t\t\t\t/usr/local/include,\n\t\t\t\t\t/usr/local/include/eigen3,\n\t\t\t\t);\n\t\t\t\tLD_RUNPATH_SEARCH_PATHS = \"\";\n\t\t\t\tLIBRARY_SEARCH_PATHS = (\n\t\t\t\t\t\"$(inherited)\",\n\t\t\t\t\t\"$(PROJECT_DIR)/GSLAM/lib\",\n\t\t\t\t\t/usr/local/lib,\n\t\t\t\t\t\"$(PROJECT_DIR)\",\n\t\t\t\t\t\"$(PROJECT_DIR)/GSLAM\",\n\t\t\t\t);\n\t\t\t\tONLY_ACTIVE_ARCH = YES;\n\t\t\t\tOTHER_LDFLAGS = \"\";\n\t\t\t\t\"OTHER_LDFLAGS[arch=*]\" = (\n\t\t\t\t\t\"-lopencv_core\",\n\t\t\t\t\t\"-lglog\",\n\t\t\t\t\t\"-lceres\",\n\t\t\t\t\t\"-lopencv_calib3d\",\n\t\t\t\t\t\"-lopencv_imgproc\",\n\t\t\t\t\t\"-ltbb\",\n\t\t\t\t\t\"-lopencv_features2d\",\n\t\t\t\t\t\"-lClp\",\n\t\t\t\t\t\"-lCoinUtils\",\n\t\t\t\t\t\"-lopencv_imgcodecs\",\n\t\t\t\t\t\"-lpangolin\",\n\t\t\t\t\t\"-lopencv_highgui\",\n\t\t\t\t\t\"-lopencv_videoio\",\n\t\t\t\t\t\"-lopencv_video\",\n\t\t\t\t\t\"-lcholmod\",\n\t\t\t\t\t\"-lDBoW2\",\n\t\t\t\t);\n\t\t\t\tPRODUCT_NAME = \"$(TARGET_NAME)\";\n\t\t\t\tSDKROOT = macosx10.13;\n\t\t\t};\n\t\t\tname = Release;\n\t\t};\n/* End XCBuildConfiguration section */\n\n/* Begin XCConfigurationList section */\n\t\t8674FBAB1D76587800FB83D1 /* Build configuration list for PBXProject \"GSLAM\" */ = {\n\t\t\tisa = XCConfigurationList;\n\t\t\tbuildConfigurations = (\n\t\t\t\t8674FBB51D76587800FB83D1 /* Debug */,\n\t\t\t\t8674FBB61D76587800FB83D1 /* Release */,\n\t\t\t);\n\t\t\tdefaultConfigurationIsVisible = 0;\n\t\t\tdefaultConfigurationName = Release;\n\t\t};\n\t\t8674FBB71D76587800FB83D1 /* Build configuration list for PBXNativeTarget \"GSLAM\" */ = {\n\t\t\tisa = XCConfigurationList;\n\t\t\tbuildConfigurations = (\n\t\t\t\t8674FBB81D76587800FB83D1 /* Debug */,\n\t\t\t\t8674FBB91D76587800FB83D1 /* Release */,\n\t\t\t);\n\t\t\tdefaultConfigurationIsVisible = 0;\n\t\t\tdefaultConfigurationName = Release;\n\t\t};\n/* End XCConfigurationList section */\n\t};\n\trootObject = 8674FBA81D76587800FB83D1 /* Project object */;\n}\n" }, { "path": "GSLAM.xcodeproj/project.xcworkspace/contents.xcworkspacedata", "content": "\n\n \n \n\n" }, { "path": "GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcschemes/GSLAM.xcscheme", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcschemes/xcschememanagement.plist", "content": "\n\n\n\n\tSchemeUserState\n\t\n\t\tGSLAM.xcscheme\n\t\t\n\t\t\torderHint\n\t\t\t0\n\t\t\n\t\n\tSuppressBuildableAutocreation\n\t\n\t\t8674FBAF1D76587800FB83D1\n\t\t\n\t\t\tprimary\n\t\t\t\n\t\t\n\t\n\n\n" }, { "path": "GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcschemes/GSLAM.xcscheme", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcschemes/xcschememanagement.plist", "content": "\n\n\n\n\tSchemeUserState\n\t\n\t\tGSLAM.xcscheme\n\t\t\n\t\t\torderHint\n\t\t\t0\n\t\t\n\t\n\tSuppressBuildableAutocreation\n\t\n\t\t8674FBAF1D76587800FB83D1\n\t\t\n\t\t\tprimary\n\t\t\t\n\t\t\n\t\n\n\n" }, { "path": "README.md", "content": "# GSLAM: Initialization-robust Monocular Visual SLAM via Global Structure-from-Motion\n\nFor more information see\n[https://frobelbest.github.io/gslam](https://frobelbest.github.io/gslam)\n\n### 1. Related Papers\n* **GSLAM: Initialization-robust Monocular Visual SLAM via Global Structure-from-Motion**, *C. Tang, O. Wang, P. Tan*, In 3DV,2017\n* **Global Structure-from-Motion by Similarity Averaging**, *Z. Cui, P. Tan*, In ICCV, 2015\n\nGet two sample sequences from [Google Drive](https://drive.google.com/file/d/1TXRg2NuiySocCsIfibEqM4CW9u8Rleq2/view?usp=sharing) .\n\n### 2. Installation\n\n\tgit clone https://github.com/frobelbest/GSLAM.git\n\n#### 2.1 Required Dependencies\n\n##### Theia Vision Library (required for global rotation averaging).\nInstall from [http://www.theia-sfm.org](http://www.theia-sfm.org)\n##### Ceres Solver (required for local and global bundle adjustment).\nInstall from [http://ceres-solver.org](http://ceres-solver.org)\n##### CLP (required for global scale and translation averaging).\nInstall from [https://projects.coin-or.org/Clp](https://projects.coin-or.org/Clp)\n##### OpenCV (required for image processing).\nInstall from [https://opencv.org](https://opencv.org)\n##### Pangolin (required for visualization).\nInstall from [https://github.com/stevenlovegrove/Pangolin](https://github.com/stevenlovegrove/Pangolin)\n\n#### 2.3 Build\nCurrently, only the xcode project is supplied. You can write your own code to compile on other platforms or wait for future update.\n\n### 3 Usage\nRun on a dataset from [Google Drive](https://drive.google.com/file/d/1TXRg2NuiySocCsIfibEqM4CW9u8Rleq2/view?usp=sharing) using\nGSLAM [sequence_path] [vocabulary_path], for example GSLAM ./robot ./Vocabulary/ORBvoc.txt\nThe 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)\n\n#### 3.1 Dataset Format.\nUnder each sequnce folder you will see the following files:\n- `shake.mov` The input video.\n- `framestamp.txt` The timestamps for each frame.\n- `gyro.txt` The gyroscope readings recorded along with the video.\n- `config.yaml` The config settings.\n\n\n### Notes\n\n#### Real-time KLT with AVX Acceleration\nExcept 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.\n\n#### ToDo\nThe main bottleneck for this project is the feature tracking, which can be further improved by the paper \"Better feature tracking through subspace constraints\".\n\n### 5 License\nGSLAM was developed at the Simon Fraser University and Adobe.\nThe open-source version is licensed under the GNU General Public License\nVersion 3 (GPLv3). For commercial purposes, please contact [cta73@sfu.ca](cta73@sfu.ca) or [pingtan@sfu.ca](pingtan@sfu.ca)\n" } ]