Showing preview only (779K chars total). Download the full file or copy to clipboard to get everything.
Repository: frobelbest/GSLAM
Branch: master
Commit: dcddfaf90644
Files: 71
Total size: 748.8 KB
Directory structure:
gitextract_116388qb/
├── GSLAM/
│ ├── Drawer copy.h
│ ├── Drawer.cpp
│ ├── Drawer.h
│ ├── Estimation.cpp
│ ├── Estimation.h
│ ├── Frame.cpp
│ ├── Frame.h
│ ├── Geometry.cpp
│ ├── Geometry.h
│ ├── GlobalReconstruction.cpp
│ ├── GlobalReconstruction.h
│ ├── GlobalRotationEstimation.cpp
│ ├── GlobalScaleEstimation.cpp
│ ├── GlobalTranslationEstimation.cpp
│ ├── Homography.hpp
│ ├── IMU.hpp
│ ├── ImageGrids.hpp
│ ├── KLT.h
│ ├── KLTUtil.cpp
│ ├── KLTUtil.h
│ ├── KeyFrame.cpp
│ ├── KeyFrame.h
│ ├── KeyFrameConnection.cpp
│ ├── KeyFrameConnection.h
│ ├── KeyFrameDatabase.cpp
│ ├── KeyFrameDatabase.h
│ ├── L1Solver.h
│ ├── LK.hpp
│ ├── MapPoint.cpp
│ ├── MapPoint.h
│ ├── ORBVocabulary.h
│ ├── ORBextractor.cc
│ ├── ORBextractor.h
│ ├── ORBmatcher.cc
│ ├── ORBmatcher.h
│ ├── RelativeMotion.hpp
│ ├── Settings.h
│ ├── System.cpp
│ ├── System.h
│ ├── TrackUtil.cpp
│ ├── TrackUtil.h
│ ├── Transform.cpp
│ ├── Transform.h
│ ├── convolve.cpp
│ ├── convolve.h
│ ├── error.cpp
│ ├── error.h
│ ├── experiment_helpers.hpp
│ ├── las2.cpp
│ ├── main.cpp
│ ├── pyramid.cpp
│ ├── pyramid.h
│ ├── random_generators.hpp
│ ├── selectGoodFeatures.hpp
│ ├── selectGoodFeatures2.hpp
│ ├── svdlib.cpp
│ ├── svdlib.hpp
│ ├── svdutil.cpp
│ ├── svdutil.hpp
│ └── time_measurement.hpp
├── GSLAM.xcodeproj/
│ ├── project.pbxproj
│ ├── project.xcworkspace/
│ │ ├── contents.xcworkspacedata
│ │ └── xcuserdata/
│ │ ├── chaos.xcuserdatad/
│ │ │ └── UserInterfaceState.xcuserstate
│ │ └── ctang.xcuserdatad/
│ │ └── UserInterfaceState.xcuserstate
│ └── xcuserdata/
│ ├── chaos.xcuserdatad/
│ │ ├── xcdebugger/
│ │ │ └── Breakpoints_v2.xcbkptlist
│ │ └── xcschemes/
│ │ ├── GSLAM.xcscheme
│ │ └── xcschememanagement.plist
│ └── ctang.xcuserdatad/
│ ├── xcdebugger/
│ │ └── Breakpoints_v2.xcbkptlist
│ └── xcschemes/
│ ├── GSLAM.xcscheme
│ └── xcschememanagement.plist
└── README.md
================================================
FILE CONTENTS
================================================
================================================
FILE: GSLAM/Drawer copy.h
================================================
//
// Drawer.h
// GSLAM
//
// Created by ctang on 11/19/16.
// Copyright © 2016 ctang. All rights reserved.
//
#ifndef DRAWER_H
#define DRAWER_H
#include "pangolin/pangolin.h"
namespace GSLAM
{
class KeyFrame;
class Drawer{
public:
float mCameraSize;
float mCameraLineWidth;
float mGraphLineWidth;
float mPointSize;
int mFrameIndex;
int mKeyFrameIndex;
std::vector<KeyFrame*> keyFrames;
void drawCurrentCamera(pangolin::OpenGlMatrix &Twc);
void getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &Twc);
void drawKeyFrames();
void drawPoints();
};
}
#include "KeyFrame.h"
namespace GSLAM{
void Drawer::drawCurrentCamera(pangolin::OpenGlMatrix &Twc) {
const float &w = mCameraSize;
const float h = w*0.75;
const float z = w*0.6;
glPushMatrix();
glMultMatrixd(Twc.m);
glLineWidth(mCameraLineWidth);
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(w,h,z);
glVertex3f(0,0,0);
glVertex3f(w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(w,-h,z);
glEnd();
glPopMatrix();
}
void Drawer::getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M){
if(mFrameIndex>=keyFrames[mKeyFrameIndex]->mvLocalFrames.back().frameId){
mKeyFrameIndex++;
}
printf("%d %d\n",mFrameIndex,keyFrames[mKeyFrameIndex]->frameId);
Transform pose;
Transform pose1;
Transform pose2
if(mKeyFrameIndex<1){
pose1=keyFrames[mKeyFrameIndex]->pose;
pose2=keyFrames[mKeyFrameIndex]->pose;
}else{
pose1.rotation=keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.rotation
*keyFrames[mKeyFrameIndex-1]->pose.rotation;
Eigen::Vector3d translation;
translation =keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.translation
/keyFrames[mKeyFrameIndex-1]->scale;
translation =keyFrames[mKeyFrameIndex-1]->pose.rotation.transpose()*translation;
pose.translation+=translation;
Transform pose2;
pose2.
}
if (mFrameIndex==keyFrames[mKeyFrameIndex]->frameId) {
}else if ((mFrameIndex-keyFrames[mKeyFrameIndex]->frameId)<keyFrames[mKeyFrameIndex]->mvCloseFrames.size()) {
/*int LocalIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId;
pose.rotation=keyFrames[mKeyFrameIndex]->mvCloseFrames[LocalIndex].pose.rotation
*keyFrames[mKeyFrameIndex]->pose.rotation;
Eigen::Vector3d translation;
translation =keyFrames[mKeyFrameIndex]->mvCloseFrames[LocalIndex].pose.translation
/keyFrames[mKeyFrameIndex]->scale;
translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation;
pose.translation+=translation;*/
}else{
int localIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId-keyFrames[mKeyFrameIndex]->mvCloseFrames.size();
pose.rotation=keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.rotation
*keyFrames[mKeyFrameIndex]->pose.rotation;
Eigen::Vector3d translation;
translation =keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.translation
/keyFrames[mKeyFrameIndex]->scale;
translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation;
pose.translation+=translation;
}
pose.rotation.transposeInPlace();
M.m[0] = pose.rotation(0,0);
M.m[1] = pose.rotation(1,0);
M.m[2] = pose.rotation(2,0);
M.m[3] = 0.0;
M.m[4] = pose.rotation(0,1);
M.m[5] = pose.rotation(1,1);
M.m[6] = pose.rotation(2,1);
M.m[7] = 0.0;
M.m[8] = pose.rotation(0,2);
M.m[9] = pose.rotation(1,2);
M.m[10] = pose.rotation(2,2);
M.m[11] = 0.0;
M.m[12] = pose.translation(0);
M.m[13] = pose.translation(1);
M.m[14] = pose.translation(2);
M.m[15] = 1.0;
mFrameIndex++;
}
void Drawer::drawPoints(){
glPointSize(4);
glBegin(GL_POINTS);
for(int k=0;k<mKeyFrameIndex;k++){
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){
Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition();
point3D/=keyFrames[k]->scale;
point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation;
uchar* color=keyFrames[k]->mvLocalMapPoints[i].color;
glColor3f(color[2]*(1.0/255.0),color[1]*(1.0/255.0),color[0]*(1.0/255.0));
glVertex3d(point3D(0),point3D(1),point3D(2));
}
}
}
glEnd();
}
void Drawer::drawKeyFrames(){
const float &w = mCameraSize;
const float h = w*0.75;
const float z = w*0.6;
for(size_t i=0; i<=mKeyFrameIndex; i++){
KeyFrame* pKF = keyFrames[i];
Transform pose=pKF->pose;
pose.rotation.transposeInPlace();
pangolin::OpenGlMatrix M;
M.m[0] = pose.rotation(0,0);
M.m[1] = pose.rotation(1,0);
M.m[2] = pose.rotation(2,0);
M.m[3] = 0.0;
M.m[4] = pose.rotation(0,1);
M.m[5] = pose.rotation(1,1);
M.m[6] = pose.rotation(2,1);
M.m[7] = 0.0;
M.m[8] = pose.rotation(0,2);
M.m[9] = pose.rotation(1,2);
M.m[10] = pose.rotation(2,2);
M.m[11] = 0.0;
M.m[12] = pose.translation(0);
M.m[13] = pose.translation(1);
M.m[14] = pose.translation(2);
M.m[15] = 1.0;
glPushMatrix();
glMultMatrixd(M.m);
glLineWidth(mCameraLineWidth*1.5);
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(w,h,z);
glVertex3f(0,0,0);
glVertex3f(w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(w,-h,z);
glEnd();
glPopMatrix();
}
glLineWidth(mGraphLineWidth);
glBegin(GL_LINES);
for(size_t k=0;k<=mKeyFrameIndex;k++)
{
// Covisibility Graph
Eigen::Vector3d center=keyFrames[k]->pose.translation;
for(map<KeyFrame*,Transform>::iterator mit=keyFrames[k]->mConnectedKeyFramePoses.begin(),
mend=keyFrames[k]->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
if(mit->first->mnId<keyFrames[k]->mnId){
if(mit->first->nextKeyFramePtr==keyFrames[k]&&mit->first==keyFrames[k]->prevKeyFramePtr){
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
Eigen::Vector3d center2=mit->first->pose.translation;
glVertex3d(center(0),center(1),center(2));
glVertex3d(center2(0),center2(1),center2(2));
}else if(mit->second.scale==-1.0){
glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0);
Eigen::Vector3d center2=mit->first->pose.translation;
glVertex3d(center(0),center(1),center(2));
glVertex3d(center2(0),center2(1),center2(2));
}else{
glColor3f(237.0f/255.0,177.0f/255.0,32.0f/255.0);
Eigen::Vector3d center2=mit->first->pose.translation;
glVertex3d(center(0),center(1),center(2));
glVertex3d(center2(0),center2(1),center2(2));
}
}
}
}
glEnd();
}
}
#endif
================================================
FILE: GSLAM/Drawer.cpp
================================================
//
// Drawer.cpp
// GSLAM
//
// Created by ctang on 11/19/16.
// Copyright © 2016 ctang. All rights reserved.
//
================================================
FILE: GSLAM/Drawer.h
================================================
//
// Drawer.h
// GSLAM
//
// Created by ctang on 11/19/16.
// Copyright © 2016 ctang. All rights reserved.
//
#ifndef DRAWER_H
#define DRAWER_H
#include "pangolin/pangolin.h"
#include "ceres/rotation.h"
#include <vector>
#include <utility>
namespace GSLAM
{
class KeyFrame;
class Drawer{
public:
float mCameraSize;
float mCameraLineWidth;
float mGraphLineWidth;
float mPointSize;
int mFrameIndex;
int mKeyFrameIndex;
std::vector<KeyFrame*> keyFrames;
void drawCurrentCamera(pangolin::OpenGlMatrix &Twc);
void drawCurrentTrajectory(pangolin::OpenGlMatrix &Twc);
void getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &Twc);
void drawKeyFrames();
void drawPoints();
std::vector<pangolin::OpenGlMatrix> preTwc;
};
}
#include "KeyFrame.h"
namespace GSLAM{
void Drawer::drawCurrentTrajectory(pangolin::OpenGlMatrix &Twc) {
const float &w = mCameraSize;
const float h = w*0.75;
const float z = w*0.6;
glPushMatrix();
glMultMatrixd(Twc.m);
glLineWidth(mCameraLineWidth);
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(w,h,z);
glVertex3f(0,0,0);
glVertex3f(w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(w,-h,z);
glEnd();
glPopMatrix();
preTwc.push_back(Twc);
glPointSize(4);
glBegin(GL_POINTS);
for(int i=0;i<preTwc.size();i++){
glVertex3d(preTwc[i].m[12],preTwc[i].m[13],preTwc[i].m[14]);
}
glEnd();
}
void Drawer::drawCurrentCamera(pangolin::OpenGlMatrix &Twc) {
const float &w = mCameraSize;
const float h = w*0.75;
const float z = w*0.6;
glPushMatrix();
glMultMatrixd(Twc.m);
glLineWidth(mCameraLineWidth);
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(w,h,z);
glVertex3f(0,0,0);
glVertex3f(w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(w,-h,z);
glEnd();
glPopMatrix();
glPointSize(4);
glBegin(GL_POINTS);
for(int i=0;i<preTwc.size();i++){
glVertex3d(preTwc[i].m[12],preTwc[i].m[13],preTwc[i].m[14]);
}
glEnd();
preTwc.push_back(Twc);
}
void Drawer::getCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M){
if(mFrameIndex>=keyFrames[mKeyFrameIndex]->mvLocalFrames.back().frameId){
preTwc.clear();
mKeyFrameIndex++;
}
//printf("%d %d\n",mFrameIndex,keyFrames[mKeyFrameIndex]->frameId);
Transform pose;
Transform pose1;
Transform pose2;
if(mKeyFrameIndex<1){
pose1=keyFrames[mKeyFrameIndex]->pose;
pose2=keyFrames[mKeyFrameIndex]->pose;
}else{
pose1.rotation=keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.rotation
*keyFrames[mKeyFrameIndex-1]->pose.rotation;
Eigen::Vector3d translation;
translation =keyFrames[mKeyFrameIndex-1]->mvLocalFrames.back().pose.translation
/keyFrames[mKeyFrameIndex-1]->scale;
translation =keyFrames[mKeyFrameIndex-1]->pose.rotation.transpose()*translation;
pose1.translation=keyFrames[mKeyFrameIndex-1]->pose.translation+translation;
pose2=keyFrames[mKeyFrameIndex]->pose;
}
if (mFrameIndex==keyFrames[mKeyFrameIndex]->frameId) {
pose=pose1;
}else if (mFrameIndex<keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId) {
double distance1=mFrameIndex-keyFrames[mKeyFrameIndex]->frameId;
double distance2=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId-mFrameIndex;
Eigen::Vector3d angle1,angle2,angle;
ceres::RotationMatrixToAngleAxis(pose1.rotation.data(),angle1.data());
ceres::RotationMatrixToAngleAxis(pose2.rotation.data(),angle2.data());
angle=(distance2*angle1+distance1*angle2)/(distance1+distance2);
ceres::AngleAxisToRotationMatrix(angle.data(),pose.rotation.data());
pose.translation=(distance2*pose1.translation+distance1*pose2.translation)/(distance1+distance2);
pose1=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].pose;
ceres::RotationMatrixToAngleAxis(pose1.rotation.data(),angle1.data());
angle1*=(distance1/(distance1+distance2));
ceres::AngleAxisToRotationMatrix(angle1.data(),pose1.rotation.data());
pose1.translation=keyFrames[mKeyFrameIndex]->mvLocalFrames[0].pose.translation/keyFrames[mKeyFrameIndex]->scale;
pose1.translation*=(distance1/(distance1+distance2));
pose.rotation=pose1.rotation*pose.rotation;
pose.translation+=pose.rotation.transpose()*pose1.translation;
}else{
int localIndex=mFrameIndex-keyFrames[mKeyFrameIndex]->mvLocalFrames[0].frameId;
pose.rotation=keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.rotation
*keyFrames[mKeyFrameIndex]->pose.rotation;
Eigen::Vector3d translation;
translation =keyFrames[mKeyFrameIndex]->mvLocalFrames[localIndex].pose.translation
/keyFrames[mKeyFrameIndex]->scale;
translation =keyFrames[mKeyFrameIndex]->pose.rotation.transpose()*translation;
pose.translation=pose2.translation+translation;
}
pose.rotation.transposeInPlace();
M.m[0] = pose.rotation(0,0);
M.m[1] = pose.rotation(1,0);
M.m[2] = pose.rotation(2,0);
M.m[3] = 0.0;
M.m[4] = pose.rotation(0,1);
M.m[5] = pose.rotation(1,1);
M.m[6] = pose.rotation(2,1);
M.m[7] = 0.0;
M.m[8] = pose.rotation(0,2);
M.m[9] = pose.rotation(1,2);
M.m[10] = pose.rotation(2,2);
M.m[11] = 0.0;
M.m[12] = pose.translation(0);
M.m[13] = pose.translation(1);
M.m[14] = pose.translation(2);
M.m[15] = 1.0;
mFrameIndex++;
}
void Drawer::drawPoints(){
glPointSize(4);
glBegin(GL_POINTS);
for(int k=0;k<mKeyFrameIndex;k++){
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){
Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition();
point3D/=keyFrames[k]->scale;
point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation;
uchar* color=keyFrames[k]->mvLocalMapPoints[i].color;
glColor3f(color[2]*(1.0/255.0),color[1]*(1.0/255.0),color[0]*(1.0/255.0));
glVertex3d(point3D(0),point3D(1),point3D(2));
}
}
}
glEnd();
}
void Drawer::drawKeyFrames(){
const float &w = mCameraSize;
const float h = w*0.75;
const float z = w*0.6;
std::vector<Eigen::Vector3d> centers(0);
for(size_t i=0; i<=mKeyFrameIndex; i++){
KeyFrame* pKF = keyFrames[i];
Transform pose;
if(i==0){
pose=pKF->pose;
}else{
pose.rotation=keyFrames[i-1]->mvLocalFrames.back().pose.rotation
*keyFrames[i-1]->pose.rotation;
Eigen::Vector3d translation;
translation =keyFrames[i-1]->mvLocalFrames.back().pose.translation/keyFrames[i-1]->scale;
translation =keyFrames[i-1]->pose.rotation.transpose()*translation;
pose.translation=keyFrames[i-1]->pose.translation+translation;
}
pose.rotation.transposeInPlace();
centers.push_back(pose.translation);
pangolin::OpenGlMatrix M;
M.m[0] = pose.rotation(0,0);
M.m[1] = pose.rotation(1,0);
M.m[2] = pose.rotation(2,0);
M.m[3] = 0.0;
M.m[4] = pose.rotation(0,1);
M.m[5] = pose.rotation(1,1);
M.m[6] = pose.rotation(2,1);
M.m[7] = 0.0;
M.m[8] = pose.rotation(0,2);
M.m[9] = pose.rotation(1,2);
M.m[10] = pose.rotation(2,2);
M.m[11] = 0.0;
M.m[12] = pose.translation(0);
M.m[13] = pose.translation(1);
M.m[14] = pose.translation(2);
M.m[15] = 1.0;
glPushMatrix();
glMultMatrixd(M.m);
glLineWidth(mCameraLineWidth*1.5);
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(w,h,z);
glVertex3f(0,0,0);
glVertex3f(w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,-h,z);
glVertex3f(0,0,0);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(w,-h,z);
glEnd();
glPopMatrix();
}
glLineWidth(mGraphLineWidth);
glBegin(GL_LINES);
//printf("show %d keyframes\n",centers.size());
std::vector<std::pair<Eigen::Vector3d,Eigen::Vector3d> > lines;
for(size_t k=0;k<=mKeyFrameIndex;k++)
{
// Covisibility Graph
Eigen::Vector3d center=centers[k];//=keyFrames[k]->pose.translation;
for(map<KeyFrame*,Transform>::iterator mit=keyFrames[k]->mConnectedKeyFramePoses.begin(),
mend=keyFrames[k]->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
if(mit->first->mnId<keyFrames[k]->mnId){
if(mit->first->nextKeyFramePtr==keyFrames[k]&&mit->first==keyFrames[k]->prevKeyFramePtr){
glColor3f(0.0f/255.0,114.0f/255.0,189.0f/255.0);
Eigen::Vector3d center2=centers[mit->first->mnId];//mit->first->pose.translation;
glVertex3d(center(0),center(1),center(2));
glVertex3d(center2(0),center2(1),center2(2));
}else if(mit->second.scale==-1.0){
Eigen::Vector3d center2=centers[mit->first->mnId];
if(mit->first->frameId<70||keyFrames[k]->frameId<70){
lines.push_back(std::make_pair(center,center2));
continue;
}
glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0);
glVertex3d(center(0),center(1),center(2));
glVertex3d(center2(0),center2(1),center2(2));
}else{
glColor3f(237.0f/255.0,177.0f/255.0,32.0f/255.0);
Eigen::Vector3d center2=centers[mit->first->mnId];//mit->first->pose.translation;
glVertex3d(center(0),center(1),center(2));
glVertex3d(center2(0),center2(1),center2(2));
}
}
}
}
glEnd();
glLineWidth(mGraphLineWidth);
glLineStipple(8, 0xAAAA);
glEnable(GL_LINE_STIPPLE);
glBegin(GL_LINES);
for(int i=0;i<lines.size();i++){
Eigen::Vector3d center1=lines[i].first;
Eigen::Vector3d center2=lines[i].second;
glColor3f(162.0f/255.0,20.0f/255.0,47.0f/255.0);
glVertex3d(center1(0),center1(1),center1(2));
glVertex3d(center2(0),center2(1),center2(2));
}
glEnd();
glDisable(GL_LINE_STIPPLE);
}
}
#endif
================================================
FILE: GSLAM/Estimation.cpp
================================================
//
// Estimation.cpp
// GSLAM
//
// Created by ctang on 9/10/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include "svdlib.hpp"
#include "svdutil.hpp"
#include "Estimation.h"
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include <iostream>
#include "Geometry.h"
#include "RelativeMotion.hpp"
#include "opencv2/calib3d/calib3d.hpp"
bool getPVector(const Eigen::Vector3d &T12,
const Eigen::Vector3d &p1,
const Eigen::Vector3d &p2,
Eigen::Vector3d &pVector){
Eigen::Vector3d T=p1;
Eigen::Vector3d r1=T12;
Eigen::Vector3d r2=-p2;
Eigen::Vector3d xij=r1.cross(r2);
//analytic solution
Eigen::Matrix3d A;
for(int i=0;i<3;i++){
A(i,0)=r1(i);
A(i,1)=-r2(i);
A(i,2)=-xij(i);
}
double determinantA=A.determinant();
Eigen::Matrix3d D=A;
D.col(0)=T;
double determinant1=D.determinant();
A.col(1)=T;
double determinant2=A.determinant();
double s_ik=determinant1/determinantA;
double s_jk=determinant2/determinantA;
if(s_ik<0||s_jk<0){
return false;
}
Eigen::Matrix3d R_jik=ComputeAxisRotation(T,r1);
Eigen::Matrix3d R_ijk=ComputeAxisRotation(-T,r2);
Eigen::Matrix3d PMatrix=(Eigen::Matrix3d::Identity()+s_ik*R_jik-s_jk*R_ijk);
pVector=PMatrix*p1;
pVector=0.5*pVector;
return true;
}
void getScaleRatio(const Eigen::Vector3d &T12,
const Eigen::Vector3d &p1,
const Eigen::Vector3d &p2,
double &s1,double &s2){
Eigen::Vector3d T=p1;
Eigen::Vector3d r1=T12;
Eigen::Vector3d r2=-p2;
Eigen::Vector3d xij=r1.cross(r2);
//analytic solution
Eigen::Matrix3d A;
for(int i=0;i<3;i++){
A(i,0)=r1(i);
A(i,1)=-r2(i);
A(i,2)=-xij(i);
}
double determinantA=A.determinant();
Eigen::Matrix3d D=A;
D.col(0)=T;
double determinant1=D.determinant();
A.col(1)=T;
double determinant2=A.determinant();
s1=determinant1/determinantA;
s2=determinant2/determinantA;
}
Eigen::Vector3d estimateRelativeTranslationIRLS(const std::vector<Eigen::Vector3d> &pts1,
const std::vector<Eigen::Vector3d> &pts2,
std::vector<Eigen::Vector3d> &pVectors,
std::vector<bool> &status){
int k = 0;
int kmax = 100;
int max_in_iter = 10;
int in_iter = 0;
double epslon = 1e-5;
int num_point=pts1.size();
Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);
for(int i=0;i<num_point;i++){
allNorms.col(i)=pts1[i].cross(pts2[i]);
allNorms.col(i).normalize();
}
Eigen::VectorXd weights=Eigen::VectorXd::Ones(num_point);
double costVal = 0.0;
Eigen::Vector3d translation=Eigen::Vector3d::Random();
translation.normalize();
while((in_iter < max_in_iter)&&(k < kmax)){
Eigen::Matrix3Xd allNormsWeighted=Eigen::Matrix3Xd(3,num_point);
for(int i=0;i<num_point;i++){
double weight=1.0/std::max(weights(i),1e-5);
allNormsWeighted.col(i)=weight*allNorms.col(i);
}
Eigen::Matrix3d NtN=allNormsWeighted*allNormsWeighted.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(NtN,Eigen::ComputeFullV);
Eigen::Matrix3d V=svd.matrixV();
Eigen::Vector3d translationNew=V.col(2);
Eigen::VectorXd weightsNew=(translationNew.transpose()*allNorms);
weights=weightsNew.cwiseAbs();
double costNew=weights.sum();
double delt = std::max(std::abs(costVal-costNew),1-translationNew.dot(translation));
if (delt <= epslon){
in_iter++;
}
else{
in_iter=0;
}
costVal=costNew;
translation=translationNew;
k++;
break;
}
pVectors.resize(num_point);
status.resize(num_point);
std::fill(status.begin(),status.end(),true);
std::vector<double> s1(num_point),s2(num_point);
for (int i=0;i<num_point;i++) {
getScaleRatio(translation,pts1[i],pts2[i],s1[i],s2[i]);
}
int overallCount=0;
int positiveCount=0;
for (int i=0;i<num_point;i++) {
if(s1[i]*s2[i]<0){
status[i]=false;
continue;
}
overallCount++;
positiveCount+=s1[i]>0;
}
bool isPositive=positiveCount>overallCount/2;
double factor=isPositive?1.0:-1.0;
translation*=factor;
for (int i=0;i<num_point;i++) {
if (status[i]==true) {
if ((s1[i]>0)==isPositive) {
double s_1=factor*s1[i];
double s_2=factor*s2[i];
Eigen::Matrix3d R_jik=ComputeAxisRotation(pts1[i],translation);
Eigen::Matrix3d R_ijk=ComputeAxisRotation(-pts1[i],-pts2[i]);
Eigen::Matrix3d PMatrix=(Eigen::Matrix3d::Identity()+s_1*R_jik-s_2*R_ijk);
pVectors[i]=0.5*PMatrix*pts1[i];
}else{
status[i]=false;
}
}
}
return translation;
}
Eigen::Vector3d estimateRelativeTranslation(const std::vector<Eigen::Vector3d> &pts1,
const std::vector<Eigen::Vector3d> &pts2,
std::vector<Eigen::Vector3d> &pVectors,
std::vector<bool> &status){
int num_point=pts1.size();
std::vector<Eigen::Vector3d> norms(num_point);
pVectors.resize(num_point);
status.resize(num_point);
Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);
Eigen::Vector3d preResult,curResult;
for(int i=0;i<num_point;i++){
norms[i]=pts1[i].cross(pts2[i]);
norms[i].normalize();
allNorms.col(i)=norms[i];
}
Eigen::Matrix3d NtN=allNorms*allNorms.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(NtN,Eigen::ComputeFullV);
Eigen::Matrix3d V=svd.matrixV();
curResult=V.col(2);
int positiveCount=0;
std::vector<int> flags(num_point);
for(int i=0;i<num_point;i++){
Eigen::Vector3d flowVector=pts1[i]-pts2[i];
flags[i]=(flowVector.dot(curResult)>0);
positiveCount+=flags[i];
}
int flag=1;
if((double)positiveCount<0.5*(double)num_point){
curResult*=-1;
flag=0;
}
for(int i=0;i<num_point;i++){
if(flags[i]!=flag){
continue;
}
status[i]=getPVector(curResult,pts1[i],pts2[i],pVectors[i]);
}
return curResult;
}
typedef struct {
bool flag;
double value;
int index;
}Residual;
typedef struct{
bool operator() (const Residual &r1,const Residual &r2) {
return r1.value<r2.value;
}
}compareResidual;
Eigen::Vector3d estimateRelativeTranslationRobust(const std::vector<Eigen::Vector3d> &pts1,
const std::vector<Eigen::Vector3d> &pts2,
std::vector<bool> &status,
const double inlierPercentage=0.8,
const double inlierThreshold=std::sin((3.0/180.0)*CV_PI)){
int num_point=pts1.size();
std::vector<Residual> residuals(num_point);
std::vector<Eigen::Vector3d> norms(num_point);
status.resize(num_point);
std::vector<Eigen::Vector3d> pVectors(num_point);
Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);
int numInliers=0;
double preError=1.0,curError=1.0;
Eigen::Vector3d preResult,curResult;
for(int iter=0;iter<10;iter++){
if(iter==0){
for(int i=0;i<num_point;i++){
norms[i]=pts1[i].cross(pts2[i]);
norms[i].normalize();
allNorms.col(i)=norms[i];
}
Eigen::Matrix3d NtN=allNorms*allNorms.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(NtN,Eigen::ComputeFullV);
Eigen::Matrix3d V=svd.matrixV();
curResult=V.col(2);
Eigen::VectorXd errors=curResult.transpose()*allNorms;
for(int i=0;i<num_point;i++){
residuals[i].index=i;
residuals[i].value=std::abs(errors(i));
}
}else{
Eigen::Matrix3Xd subNorms=Eigen::Matrix3Xd(3,numInliers);
for(int i=0;i<numInliers;i++){
int index=residuals[i].index;
subNorms.col(i)=norms[index];
}
Eigen::Matrix3d NtN=subNorms*subNorms.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(NtN,Eigen::ComputeFullV);
Eigen::Matrix3d V=svd.matrixV();
curResult=V.col(2);
Eigen::VectorXd errors=curResult.transpose()*allNorms;
for(int i=0;i<num_point;i++){
int index=residuals[i].index;
residuals[i].value=std::abs(errors(index));
}
}
std::sort(residuals.begin(),residuals.end(),compareResidual());
int medInliers=residuals.size()*inlierPercentage;
curError=residuals[medInliers].value;
if(curError<preError){
preResult=curResult;
preError=curError;
numInliers=medInliers;
}else{
break;
}
while(numInliers<residuals.size()&&residuals[numInliers].value<inlierThreshold){
numInliers++;
}
}
std::fill(status.begin(),status.end(),false);
int positiveCount=0;
for(int i=0;i<numInliers;i++){
int index=residuals[i].index;
Eigen::Vector3d flowVector=pts1[index]-pts2[index];
residuals[i].flag=(flowVector.dot(preResult)>0);
positiveCount+=residuals[i].flag;
}
int flag=1;
if((double)positiveCount<0.5*(double)numInliers){
preResult*=-1;
flag=0;
}
for(int i=0;i<numInliers;i++){
if(residuals[i].flag!=flag){
continue;
}
int index=residuals[i].index;
status[index]=getPVector(preResult,pts1[index],pts2[index],pVectors[index]);
}
return preResult;
}
Eigen::Vector3d estimateRelativeTranslationRobust(const std::vector<Eigen::Vector3d> &pts1,
const std::vector<Eigen::Vector3d> &pts2,
std::vector<Eigen::Vector3d> &pVectors,
std::vector<bool> &status,
std::vector<double> &errors,
double &medError,
const double inlierPercentage=0.8,
const double inlierThreshold=std::sin((3.0/180.0)*CV_PI)){
int num_point=pts1.size();
std::vector<Residual> residuals(num_point);
std::vector<Eigen::Vector3d> norms(num_point);
pVectors.resize(num_point);
status.resize(num_point);
errors.resize(num_point);
Eigen::Matrix3Xd allNorms=Eigen::Matrix3Xd(3,num_point);
int numInliers=0;
double preError=1.0,curError=1.0;
Eigen::Vector3d preResult,curResult;
for(int iter=0;iter<10;iter++){
if(iter==0){
for(int i=0;i<num_point;i++){
norms[i]=pts1[i].cross(pts2[i]);
norms[i].normalize();
allNorms.col(i)=norms[i];
}
Eigen::Matrix3d NtN=allNorms*allNorms.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(NtN,Eigen::ComputeFullV);
Eigen::Matrix3d V=svd.matrixV();
curResult=V.col(2);
Eigen::VectorXd errors=curResult.transpose()*allNorms;
for(int i=0;i<num_point;i++){
residuals[i].index=i;
residuals[i].value=std::abs(errors(i));
}
}else{
Eigen::Matrix3Xd subNorms=Eigen::Matrix3Xd(3,numInliers);
for(int i=0;i<numInliers;i++){
int index=residuals[i].index;
subNorms.col(i)=norms[index];
}
Eigen::Matrix3d NtN=subNorms*subNorms.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(NtN,Eigen::ComputeFullV);
Eigen::Matrix3d V=svd.matrixV();
curResult=V.col(2);
Eigen::VectorXd errors=curResult.transpose()*allNorms;
for(int i=0;i<num_point;i++){
int index=residuals[i].index;
residuals[i].value=std::abs(errors(index));
}
}
std::sort(residuals.begin(),residuals.end(),compareResidual());
int medInliers=residuals.size()*inlierPercentage;
curError=residuals[medInliers].value;
if(curError<preError){
preResult=curResult;
preError=curError;
numInliers=medInliers;
}else{
break;
}
while(numInliers<residuals.size()&&residuals[numInliers].value<inlierThreshold){
numInliers++;
}
}
for(int i=0;i<num_point;i++){
errors[residuals[i].index]=residuals[i].value;
}
std::fill(status.begin(),status.end(),false);
int positiveCount=0;
for(int i=0;i<numInliers;i++){
int index=residuals[i].index;
Eigen::Vector3d flowVector=pts1[index]-pts2[index];
residuals[i].flag=(flowVector.dot(preResult)>0);
positiveCount+=residuals[i].flag;
}
int flag=1;
if((double)positiveCount<0.5*(double)numInliers){
preResult*=-1;
flag=0;
}
for(int i=0;i<numInliers;i++){
if(residuals[i].flag!=flag){
continue;
}
int index=residuals[i].index;
status[index]=getPVector(preResult,pts1[index],pts2[index],pVectors[index]);
}
medError=curError;
return preResult;
}
void estimateRelativeMotion(const std::vector<Eigen::Vector3d> &pts1,
std::vector<Eigen::Vector3d> &pts2,
Eigen::Matrix3d &rotation,
Eigen::Vector3d &translation,
std::vector<Eigen::Vector3d> &pVectors,
std::vector<bool> &status){
estimateRotationTranslation(0.005,
pts1,
pts2,
rotation,
translation);
//estimateRotationTranslation2(0.005,pts1,pts2,rotation,translation);
translation=rotation.transpose() * translation;
translation.normalize();
for (int i=0;i<pts2.size();i++) {
pts2[i]=rotation.transpose()*pts2[i];
pts2[i].normalize();
}
int num_point=pts1.size();
int positiveCount=0;
std::vector<int> flags(num_point);
for(int i=0;i<num_point;i++){
Eigen::Vector3d flowVector=pts1[i]-pts2[i];
flags[i]=(flowVector.dot(translation)>0);
positiveCount+=flags[i];
}
int flag=1;
if((double)positiveCount<0.5*(double)num_point){
translation*=-1;
flag=0;
}
status.resize(num_point);
std::fill(status.begin(),status.end(),false);
pVectors.resize(num_point);
for(int i=0;i<num_point;i++){
if(flags[i]!=flag){
continue;
}
status[i]=getPVector(translation,pts1[i],pts2[i],pVectors[i]);
}
}
struct SnavelyReprojectionError {
SnavelyReprojectionError(const double _key_x,const double _key_y,
const double _tracked_x,const double _tracked_y,const double _weight):
key_x(_key_x),key_y(_key_y),
tracked_x(_tracked_x), tracked_y(_tracked_y),
weight(_weight){
}
template <typename T>
bool operator()(const T* const camera,
const T* const depth,
T* residuals) const {
T p[3];
T point[3]={((T)key_x)/depth[0],((T)key_y)/depth[0],1.0/depth[0]};
ceres::AngleAxisRotatePoint(camera,point,p);
p[0]+=camera[3];
p[1]+=camera[4];
p[2]+=camera[5];
T predicted_x=p[0]/p[2];
T predicted_y=p[1]/p[2];
residuals[0]=predicted_x-(T)tracked_x;
residuals[1]=predicted_y-(T)tracked_y;
residuals[0]=residuals[0]*weight;
residuals[1]=residuals[1]*weight;
return true;
}
static ceres::CostFunction* Create(const double key_x,
const double key_y,
const double tracked_x,
const double tracked_y,
const double weight=1.0) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,6,1>(new SnavelyReprojectionError(key_x,key_y,tracked_x,tracked_y,weight)));
};
double tracked_x;
double tracked_y;
double key_x;
double key_y;
double weight;
};
struct SnavelyReprojectionErrorSimple {
SnavelyReprojectionErrorSimple(
const double _key_x,const double _key_y,
const double _tracked_x,const double _tracked_y):
key_x(_key_x),key_y(_key_y),
tracked_x(_tracked_x), tracked_y(_tracked_y){
}
template <typename T>
bool operator()(const T* const camera,
const T* const depth,
T* residuals) const {
T ax=key_x-camera[2]*key_y+camera[1];
T bx=camera[3];
T ay=key_y-camera[0]+camera[2]*key_x;
T by=camera[4];
T c =-camera[1]*key_x+camera[0]*key_y+1.0;
T d =camera[5];
T ex=tracked_x*c-ax;
T ey=tracked_y*c-ay;
if(depth[0]<0.01){
residuals[0]=ex;
residuals[1]=ey;
return true;
}
T fx=tracked_x*d-bx;
T fy=tracked_y*d-by;
residuals[0]=(ex+fx*depth[0])/(c+d*depth[0]);
residuals[1]=(ey+fy*depth[0])/(c+d*depth[0]);
return true;
}
static ceres::CostFunction* Create(
const double key_x,
const double key_y,
const double tracked_x,
const double tracked_y) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionErrorSimple,2,6,1>(new SnavelyReprojectionErrorSimple(key_x,key_y,tracked_x,tracked_y)));
}
double tracked_x;
double tracked_y;
double key_x;
double key_y;
};
void setSmallRotationMatrix(const double *w,Eigen::Matrix3d &rotation){
rotation(0,0)=rotation(1,1)=rotation(2,2)=1.0;
rotation(0,1)=-w[2];rotation(0,2)=w[1];
rotation(1,0)=w[2];rotation(1,2)=-w[0];
rotation(2,0)=-w[1];rotation(2,1)=w[0];
}
namespace GSLAM{
int RelativeOutlierRejection::relativeRansac(const KeyFrame *keyFrame,const KLT_FeatureList featureList){
assert(keyFrame->mvLocalMapPoints.size()==featureList->nFeatures);
vector<cv::Point2d> points1,points2;
vector<int> indices;
points1.reserve(featureList->nFeatures);
points2.reserve(featureList->nFeatures);
indices.reserve(featureList->nFeatures);
int medViewAngleCount=0,overallCount=0;
Eigen::Matrix3d rotationTranspoed=rotation.transpose();
for (int i=0;i<featureList->nFeatures;i++) {
if(featureList->feature[i]->val==KLT_TRACKED&&(*isValid)[i]){
overallCount++;
Eigen::Vector3d p1=keyFrame->mvLocalMapPoints[i].norm;
Eigen::Vector3d p2=rotationTranspoed*featureList->feature[i]->norm;
p2.normalize();
double viewAngle=p1.dot(p2);
if (viewAngle>=minViewAngle) {
(*isValid)[i]=false;
continue;
}
if (viewAngle<medViewAngle) {
medViewAngleCount++;
}
p1=K*p1;
p2=K*p2;
points1.push_back(cv::Point2d(p1(0)/p1(2),p1(1)/p1(2)));
points2.push_back(cv::Point2d(p2(0)/p2(2),p2(1)/p2(2)));
indices.push_back(i);
}
}
if (medViewAngleCount<0.5*overallCount) {
return -1;
}
points1.shrink_to_fit();
points2.shrink_to_fit();
indices.shrink_to_fit();
vector<uchar> status;
cv::findFundamentalMat(points1,points2,cv::FM_RANSAC,theshold,prob,status);
int inlierCount=status.size();
for (int i=0;i<status.size();i++) {
if (status[i]==0) {
int index=indices[i];
(*isValid)[index]=false;
inlierCount--;
}
}
return inlierCount;
}
int RelativePoseEstimation::estimateRelativePose(const KeyFrame *keyFrame,
const KLT_FeatureList featureList,
std::vector<Eigen::Vector3d*> &pVectors){
std::vector<double> viewAngles(keyFrame->mvLocalMapPoints.size());
std::fill(viewAngles.begin(),viewAngles.end(),-1.0);
pVectors.resize(keyFrame->mvLocalMapPoints.size());
std::fill(pVectors.begin(),pVectors.end(),static_cast<Eigen::Vector3d*>(NULL));
std::vector<Eigen::Vector3d> norms1,norms2;
std::vector<int> indices;
indices.reserve(keyFrame->mvLocalMapPoints.size());
norms1.reserve(keyFrame->mvLocalMapPoints.size());
norms2.reserve(keyFrame->mvLocalMapPoints.size());
for (int i=0;i<keyFrame->mvLocalMapPoints.size();i++) {
if (featureList->feature[i]->val==KLT_TRACKED&&(*isValid)[i]) {
Eigen::Vector3d norm2=rotation.transpose()*featureList->feature[i]->norm;
norm2.normalize();
norms1.push_back(keyFrame->mvLocalMapPoints[i].norm);
norms2.push_back(norm2);
indices.push_back(i);
}
}
norms1.shrink_to_fit();
norms2.shrink_to_fit();
indices.shrink_to_fit();
std::vector<bool> status;
std::vector<Eigen::Vector3d> tmpPVectors;
translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status);
//translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status);
//Eigen::Matrix3d inputRotation;
//Eigen::Vector3d inputTranslation;
//this->rotation=Eigen::Matrix3d::Identity();
//this->translation=Eigen::Vector3d::Zero();
//estimateRelativeMotion(norms1,norms2,this->rotation,this->translation,tmpPVectors,status);
/*Eigen::Matrix3d transposedRotation=rotation.transpose();
for (int i=0;i<norms2.size();i++) {
norms2[i]=transposedRotation*norms2[i];
}
translation=estimateRelativeTranslation(norms1,norms2,tmpPVectors,status);*/
int successCount=0;
for (int i=0;i<status.size();i++) {
int ind=indices[i];
if (status[i]==true) {
pVectors[ind]=new Eigen::Vector3d();
*pVectors[ind]= tmpPVectors[i];
successCount++;
}else{
//printf("%d %d\n",ind,i);
}
}
return successCount;
}
void LocalFactorization::process(KeyFrame* keyFrame){
std::vector<int> indices(0);
for(int i=0;i<keyFrame->mvLocalMapPoints.size();i++){
keyFrame->mvLocalMapPoints[i].isFullTrack&=(!keyFrame->mvLocalMapPoints[i].vecs.empty());
if(keyFrame->mvLocalMapPoints[i].isFullTrack){
indices.push_back(i);
}
}
int num_frame=keyFrame->mvLocalFrames.size();
int num_point=indices.size();
//printf("%d %d\n",num_frame,num_point);getchar();
DMat denseMatrix=svdNewDMat(num_point,3*num_frame);
for(int p=0;p<num_point;p++){
int ind=indices[p];
const std::vector<Eigen::Vector3d*>& pVectors=keyFrame->mvLocalMapPoints[ind].pVectors;
for(int f=0;f<num_frame;f++){
memcpy(&denseMatrix->value[p][3*f],&(*pVectors[f])(0),3*sizeof(double));
}
}
SMat sparseMatrix=svdConvertDtoS(denseMatrix);
SVDRec svdResult=svdLAS2A(sparseMatrix,12);
#ifdef DEBUG
printf("singular %f %f\n",svdResult->S[0],svdResult->S[1]);
#endif
svdFreeDMat(denseMatrix);
svdFreeSMat(sparseMatrix);
int positiveCount=0;
for(int p=0;p<num_point;p++){
positiveCount+=(svdResult->Ut->value[0][p]>0);
}
if(positiveCount<num_point/2){
positiveCount=-1;
}else{
positiveCount=1;
}
//convert results to 3d coordinates
for(int p=0;p<num_point;p++){
svdResult->Ut->value[0][p]*=positiveCount;
int ind=indices[p];
if (svdResult->Ut->value[0][p]>0) {
keyFrame->mvLocalMapPoints[ind].invdepth=svdResult->Ut->value[0][p]/keyFrame->mvLocalMapPoints[ind].norm(2);
keyFrame->mvLocalMapPoints[ind].isEstimated=true;
}
}
for(int f=0;f<num_frame;f++){
keyFrame->mvLocalFrames[f].pose.translation=Eigen::Vector3d(&svdResult->Vt->value[0][3*f]);
keyFrame->mvLocalFrames[f].pose.translation*=positiveCount*svdResult->S[0];
}
}
void LocalFactorization::iterativeRefine(KeyFrame *keyFrame){
std::vector<int> indices(0);
for(int i=0;i<keyFrame->mvLocalMapPoints.size();i++){
keyFrame->mvLocalMapPoints[i].isFullTrack&=(!keyFrame->mvLocalMapPoints[i].vecs.empty());
if(keyFrame->mvLocalMapPoints[i].isFullTrack){
indices.push_back(i);
}
}
std::vector<cv::Point2d> vecs1(indices.size());
std::vector<cv::Point2d> vecs2(indices.size());
/*for (int i=0;i<vecs0.size();i++) {
Eigen::Vector3d vec=keyFrame->mvLocalMapPoints[indices[i]].vec;
vecs0[i].x=vec(0);
vecs0[i].y=vec(1);
}*/
std::ofstream of("/Users/ctang/Desktop/debug/res.txt");
//double converge=0.002;
for (int f=keyFrame->mvLocalFrames.size()-5;f<keyFrame->mvLocalFrames.size()-4;f++) {
for (int i=0;i<indices.size();i++) {
Eigen::Vector3d point3d=keyFrame->mvLocalMapPoints[indices[i]].getPosition();
Eigen::Vector3d vec1=keyFrame->mvLocalFrames[f].pose.rotation*(point3d-keyFrame->mvLocalFrames[f].pose.translation);
vec1/=vec1(2);
Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[indices[i]].vecs[f];
vecs1[i].x=vec1(0);
vecs1[i].y=vec1(1);
vecs2[i].x=vec2(0);
vecs2[i].y=vec2(1);
}
std::vector<uchar> statuts;
cv::Mat homography=cv::findHomography(vecs1,vecs2,CV_RANSAC,0.005,statuts);
std::vector<cv::Mat> rotation(1),translation(1),normal(1);
cv::Mat calib=cv::Mat::eye(3,3,CV_64FC1);
cv::decomposeHomographyMat(homography,calib,
rotation,translation,normal);
/*printf("%d\n",f);
for (int i1=0;i1<3;i1++) {
for (int i2=0;i2<3;i2++) {
printf("%f ",rotation[0].at<double>(i1,i2));
}
printf("\n");
}*/
//printf("%d\n",rotation.size());
//getchar();
for (int i=0;i<indices.size();i++) {
if (statuts[i]==0) {
continue;
}
cv::Mat vec1(3,1,CV_64FC1);
vec1.at<double>(0)=vecs1[i].x;
vec1.at<double>(1)=vecs1[i].y;
vec1.at<double>(2)=1.0;
double diffx=vec1.at<double>(0)-vecs2[i].x;
double diffy=vec1.at<double>(1)-vecs2[i].y;
of<<sqrt(diffx*diffx+diffy*diffy)<<' ';
for (int j=0;j<4;j++) {
cv::Mat _vec1=rotation[j]*vec1;
_vec1/=_vec1.at<double>(2);
diffx=_vec1.at<double>(0)-vecs2[i].x;
diffy=_vec1.at<double>(1)-vecs2[i].y;
of<<sqrt(diffx*diffx+diffy*diffy)<<' ';
}
of<<endl;
}
getchar();
}
of.close();
}
void LocalBundleAdjustment::bundleAdjust(KeyFrame *keyFrame,bool cameraFixed){
int num_point=0;
std::vector<int> indices;
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
if (keyFrame->mvLocalMapPoints[p].isEstimated) {
indices.push_back(p);
num_point++;
}
}
int num_frame=keyFrame->mvLocalFrames.size();
std::vector<int> pointNumberInFrames(num_frame,num_point);
std::vector<double> param_cam(6*num_frame),param_invDepth(num_point);
ceres::Problem problem;
printf("%d %d\n",num_point,num_frame);
for(int p=0;p<num_point;p++){
int index=indices[p];
param_invDepth[p]=keyFrame->mvLocalMapPoints[index].invdepth;
}
for(int f=0;f<num_frame;f++){
ceres::RotationMatrixToAngleAxis(&keyFrame->mvLocalFrames[f].pose.rotation(0),¶m_cam[6*f]);
Eigen::Vector3d translation=keyFrame->mvLocalFrames[f].pose.rotation*keyFrame->mvLocalFrames[f].pose.translation;
param_cam[6*f+3]=-translation(0);
param_cam[6*f+4]=-translation(1);
param_cam[6*f+5]=-translation(2);
}
ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005);
for(int p=0;p<num_point;p++){
int index=indices[p];
Eigen::Vector3d vec1=keyFrame->mvLocalMapPoints[index].vec;
for(int f=0;f<num_frame;f++){
if(keyFrame->mvLocalMapPoints[index].vecs[f]==NULL){
continue;
}
Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[index].vecs[f];
ceres::CostFunction* cost_function =SnavelyReprojectionError::Create(vec1(0),vec1(1),vec2(0),vec2(1));
problem.AddResidualBlock(cost_function,loss_function,¶m_cam[6*f],¶m_invDepth[p]);
}
}
if (cameraFixed) {
for(int f=0;f<num_frame;f++){
problem.SetParameterBlockConstant(¶m_cam[6*f]);
}
}
ceres::Solver::Options options;
options.num_threads=1;
options.max_num_iterations=5;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = false;
options.logging_type=ceres::SILENT;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
for(int f=0;f<num_frame;f++){
//printf("%d\n",f);
//Eigen::Matrix3d rotation=keyFrame->mvLocalFrames[f].pose.rotation;
ceres::AngleAxisToRotationMatrix(¶m_cam[6*f],&keyFrame->mvLocalFrames[f].pose.rotation(0));
//rotation=rotation.transpose()*keyFrame->mvLocalFrames[f].pose.rotation;
//std::cout<<rotation<<endl;
keyFrame->mvLocalFrames[f].pose.translation=-Eigen::Vector3d(¶m_cam[6*f+3]);
keyFrame->mvLocalFrames[f].pose.translation=keyFrame->mvLocalFrames[f].pose.rotation.transpose()
*keyFrame->mvLocalFrames[f].pose.translation;
keyFrame->mvLocalFrames[f].pose.setEssentialMatrix();
}
for(int p=0;p<num_point;p++){
int index=indices[p];
keyFrame->mvLocalMapPoints[index].invdepth=param_invDepth[p];
keyFrame->mvLocalMapPoints[index].updateNormalAndDepth();
}
for(int p=0;p<num_point;p++){
int index=indices[p];
Eigen::Vector3d point3D=keyFrame->mvLocalMapPoints[index].getPosition();
if(keyFrame->mvLocalMapPoints[index].errors.empty()){
keyFrame->mvLocalMapPoints[index].errors.resize(num_frame);
}
for (int f=0;f<num_frame;f++) {
if(keyFrame->mvLocalMapPoints[index].vecs[f]==NULL){
continue;
}
Eigen::Vector3d projected=keyFrame->mvLocalFrames[f].pose.rotation
*(point3D-keyFrame->mvLocalFrames[f].pose.translation);
projected/=projected(2);
projected-=(*keyFrame->mvLocalMapPoints[index].vecs[f]);
double error=projected.norm();
keyFrame->mvLocalMapPoints[index].errors[f]=error;
if (error>projErrorThres) {
delete keyFrame->mvLocalMapPoints[index].vecs[f];
keyFrame->mvLocalMapPoints[index].vecs[f]=NULL;
keyFrame->mvLocalMapPoints[index].measurementCount--;
keyFrame->mvLocalFrames[f].measurementCount--;
}
}
//assert(keyFrame->mvLocalMapPoints[index].measurementCount>0);
if (keyFrame->mvLocalMapPoints[index].measurementCount==0||keyFrame->mvLocalMapPoints[index].invdepth<0) {
keyFrame->mvLocalMapPoints[index].isEstimated=false;
}
}
/*for (int i=0;i<keyFrame->mvLocalMapPoints.size();i++) {
}*/
int measurementCountByPoint=0,measurementCountByFrame=0;
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;
}
for (int f=0;f<keyFrame->mvLocalFrames.size();f++) {
measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;
}
printf("%d %d\n",measurementCountByPoint,measurementCountByFrame);
assert(measurementCountByPoint==measurementCountByFrame);
}
void LocalBundleAdjustment::triangulate(KeyFrame* keyFrame){
int num_frame=keyFrame->mvLocalFrames.size();
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
if(!keyFrame->mvLocalMapPoints[p].isEstimated&&keyFrame->mvLocalMapPoints[p].isValid()){
std::vector<Eigen::Vector3d> normalized;
std::vector<Eigen::Matrix3d> rotations;
std::vector<Eigen::Vector3d> cameras;
normalized.reserve(num_frame);
rotations.reserve(num_frame);
cameras.reserve(num_frame);
normalized.push_back(keyFrame->mvLocalMapPoints[p].norm);
rotations.push_back(Eigen::Matrix3d::Identity());
cameras.push_back(Eigen::Vector3d::Zero());
for (int f=0;f<num_frame;f++) {
if (keyFrame->mvLocalMapPoints[p].vecs[f]==NULL) {
continue;
}
Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[p].vecs[f];
Eigen::Vector3d norm2=keyFrame->mvLocalFrames[f].pose.rotation.transpose()*vec2;
norm2.normalize();
double viewAngle=keyFrame->mvLocalMapPoints[p].norm.dot(norm2);
if (viewAngle>viewAngleThres) {
delete keyFrame->mvLocalMapPoints[p].vecs[f];
keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;
keyFrame->mvLocalMapPoints[p].measurementCount--;
keyFrame->mvLocalFrames[f].measurementCount--;
}else{
Eigen::Vector3d epiolarLine=keyFrame->mvLocalFrames[f].pose.E*keyFrame->mvLocalMapPoints[p].vec;
double squareError=epiolarLine.dot(vec2);
squareError*=squareError;
squareError/=(epiolarLine(0)*epiolarLine(0)+epiolarLine(1)*epiolarLine(1));
if (squareError>4*projErrorThres*projErrorThres) {
delete keyFrame->mvLocalMapPoints[p].vecs[f];
keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;
keyFrame->mvLocalMapPoints[p].measurementCount--;
keyFrame->mvLocalFrames[f].measurementCount--;
}else{
normalized.push_back(norm2);
rotations.push_back(Eigen::Matrix3d::Identity());
cameras.push_back(keyFrame->mvLocalFrames[f].pose.translation);
}
}
}
normalized.shrink_to_fit();
rotations.shrink_to_fit();
cameras.shrink_to_fit();
if (cameras.size()>1) {
Eigen::Vector3d point3D=multiviewTriangulationLinear(normalized,rotations,cameras);
bool isOutlier=false;
for(int i=1;i<cameras.size();i++){
Eigen::Vector3d project=point3D-cameras[i];
project/=project(2);
Eigen::Vector3d error=project-normalized[i]/normalized[i](2);
if (error.norm()>2*projErrorThres) {
isOutlier=true;
break;
}
}
if (point3D(2)<0) {
isOutlier=true;
}
if (!isOutlier) {
keyFrame->mvLocalMapPoints[p].isEstimated=true;
keyFrame->mvLocalMapPoints[p].invdepth=1.0/point3D(2);
}else{
for (int f=0;f<num_frame;f++) {
if (keyFrame->mvLocalMapPoints[p].vecs[f]!=NULL) {
delete keyFrame->mvLocalMapPoints[p].vecs[f];
keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;
keyFrame->mvLocalFrames[f].measurementCount--;
}
}
keyFrame->mvLocalMapPoints[p].measurementCount=0;
}
}
}
}
//zheli
int measurementCountByPoint=0,measurementCountByFrame=0;
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;
}
for (int f=0;f<keyFrame->mvLocalFrames.size();f++) {
measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;
}
assert(measurementCountByPoint==measurementCountByFrame);
int effectiveCount=0;
for(int i=0;i<keyFrame->mvLocalMapPoints.size();i++){
effectiveCount+=keyFrame->mvLocalMapPoints[i].isValid()==true;
}
printf("%d\n",effectiveCount);
}
//refine points by projection error
void LocalBundleAdjustment::refinePoints(KeyFrame *keyFrame){
int num_point=0;
std::vector<int> indices;
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
if (keyFrame->mvLocalMapPoints[p].isEstimated) {
indices.push_back(p);
num_point++;
}
}
int num_frame=keyFrame->mvLocalFrames.size();
for(int p=0;p<num_point;p++){
int index=indices[p];
Eigen::Vector3d point3D=keyFrame->mvLocalMapPoints[index].getPosition();
if(keyFrame->mvLocalMapPoints[index].errors.empty()){
keyFrame->mvLocalMapPoints[index].errors.resize(num_frame);
}
for (int f=0;f<num_frame;f++) {
if(keyFrame->mvLocalMapPoints[index].vecs[f]==NULL){
continue;
}
Eigen::Vector3d projected=keyFrame->mvLocalFrames[f].pose.rotation
*(point3D-keyFrame->mvLocalFrames[f].pose.translation);
projected/=projected(2);
projected-=(*keyFrame->mvLocalMapPoints[index].vecs[f]);
double error=projected.norm();
keyFrame->mvLocalMapPoints[index].errors[f]=error;
if (error>projErrorThres) {
delete keyFrame->mvLocalMapPoints[index].vecs[f];
keyFrame->mvLocalMapPoints[index].vecs[f]=NULL;
keyFrame->mvLocalMapPoints[index].measurementCount--;
keyFrame->mvLocalFrames[f].measurementCount--;
}
}
if (keyFrame->mvLocalMapPoints[index].measurementCount==0
||keyFrame->mvLocalMapPoints[index].invdepth<0) {
keyFrame->mvLocalMapPoints[index].isEstimated=false;
}
}
int measurementCountByPoint=0,measurementCountByFrame=0;
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;
}
for (int f=0;f<keyFrame->mvLocalFrames.size();f++) {
measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;
}
printf("%d %d\n",measurementCountByPoint,measurementCountByFrame);
assert(measurementCountByPoint==measurementCountByFrame);
}
void LocalBundleAdjustment::triangulate2(KeyFrame* keyFrame){
int num_frame=keyFrame->mvLocalFrames.size();
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
if(!keyFrame->mvLocalMapPoints[p].isEstimated&&keyFrame->mvLocalMapPoints[p].isValid()){
std::vector<Eigen::Vector3d> normalized;
std::vector<Eigen::Matrix3d> rotations;
std::vector<Eigen::Vector3d> cameras;
normalized.reserve(num_frame);
rotations.reserve(num_frame);
cameras.reserve(num_frame);
normalized.push_back(keyFrame->mvLocalMapPoints[p].norm);
rotations.push_back(Eigen::Matrix3d::Identity());
cameras.push_back(Eigen::Vector3d::Zero());
for (int f=0;f<num_frame;f++) {
if (keyFrame->mvLocalMapPoints[p].vecs[f]==NULL) {
continue;
}
Eigen::Vector3d vec2=*keyFrame->mvLocalMapPoints[p].vecs[f];
Eigen::Vector3d norm2=keyFrame->mvLocalFrames[f].pose.rotation.transpose()*vec2;
norm2.normalize();
double viewAngle=keyFrame->mvLocalMapPoints[p].norm.dot(norm2);
if (viewAngle>viewAngleThres) {
delete keyFrame->mvLocalMapPoints[p].vecs[f];
keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;
keyFrame->mvLocalMapPoints[p].measurementCount--;
keyFrame->mvLocalFrames[f].measurementCount--;
}else{
Eigen::Vector3d epiolarLine=keyFrame->mvLocalFrames[f].pose.E
*keyFrame->mvLocalMapPoints[p].vec;
double squareError=epiolarLine.dot(vec2);
squareError*=squareError;
squareError/=(epiolarLine(0)*epiolarLine(0)+epiolarLine(1)*epiolarLine(1));
if (squareError>4*projErrorThres*projErrorThres) {
delete keyFrame->mvLocalMapPoints[p].vecs[f];
keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;
keyFrame->mvLocalMapPoints[p].measurementCount--;
keyFrame->mvLocalFrames[f].measurementCount--;
}else{
normalized.push_back(norm2);
rotations.push_back(Eigen::Matrix3d::Identity());
cameras.push_back(keyFrame->mvLocalFrames[f].pose.translation);
}
}
}
normalized.shrink_to_fit();
rotations.shrink_to_fit();
cameras.shrink_to_fit();
if (cameras.size()>1) {
Eigen::Vector3d point3D=multiviewTriangulationLinear(normalized,rotations,cameras);
//double depth=multiviewTriangulationDepth(normalized,rotations,cameras);
//std::cout<<depth<<std::endl;
//getchar();
//Eigen::Vector3d point3D=depth*normalized[0];
bool isOutlier=false;
for(int i=1;i<cameras.size();i++){
Eigen::Vector3d project=point3D-cameras[i];
project/=project(2);
Eigen::Vector3d error=project-normalized[i]/normalized[i](2);
if (error.norm()>2*projErrorThres) {
isOutlier=true;
break;
}
}
if (point3D(2)<0) {
isOutlier=true;
}
if (!isOutlier) {
keyFrame->mvLocalMapPoints[p].isEstimated=true;
keyFrame->mvLocalMapPoints[p].invdepth=1.0/point3D(2);
}else{
for (int f=0;f<num_frame;f++) {
if (keyFrame->mvLocalMapPoints[p].vecs[f]!=NULL) {
delete keyFrame->mvLocalMapPoints[p].vecs[f];
keyFrame->mvLocalMapPoints[p].vecs[f]=NULL;
keyFrame->mvLocalFrames[f].measurementCount--;
}
}
keyFrame->mvLocalMapPoints[p].measurementCount=0;
}
}
}
}
int measurementCountByPoint=0,measurementCountByFrame=0;
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
measurementCountByPoint+=keyFrame->mvLocalMapPoints[p].measurementCount;
}
for (int f=0;f<keyFrame->mvLocalFrames.size();f++) {
measurementCountByFrame+=keyFrame->mvLocalFrames[f].measurementCount;
}
assert(measurementCountByPoint==measurementCountByFrame);
int effectiveCount=0;
for(int i=0;i<keyFrame->mvLocalMapPoints.size();i++){
effectiveCount+=keyFrame->mvLocalMapPoints[i].isValid()==true;
}
printf("%d\n",effectiveCount);
}
/*void LocalBundleAdjustment::estimateFarFrames(KeyFrame* keyFrame){
for (int i=0;i<keyFrame->mvFarFrames.size();i++) {
std::vector<Eigen::Vector3d*>& closeMeasurements=keyFrame->mvFarMeasurements[i];
std::vector<cv::Point3f> points3d;
std::vector<cv::Point2f> points2d;
for (int m=0;m<closeMeasurements.size();m++) {
if (closeMeasurements[m]!=NULL&&keyFrame->mvLocalMapPoints[m].measurementCount>0) {
cv::Point3f point3d;
cv::Point2f point2d;
Eigen::Vector3d point3D=keyFrame->mvLocalMapPoints[m].getPosition();
point3d.x=point3D(0);
point3d.y=point3D(1);
point3d.z=point3D(2);
Eigen::Vector3d normalized3d=*closeMeasurements[m];
point2d.x=normalized3d(0)/normalized3d(2);
point2d.y=normalized3d(1)/normalized3d(2);
points3d.push_back(point3d);
points2d.push_back(point2d);
//printf("3d %f %f %f %f %f\n",point3d.x,point3d.y,point3d.z,point2d.x,point2d.y);
}
}
assert(points3d.size()>10);
cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1);
cv::Mat rVec,tVec,rMat;
cv::solvePnPRansac(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec,false,1000,0.008,0.99);
cv::Rodrigues(rVec,rMat);
Transform pose;
for(int r1=0;r1<3;r1++){
for(int r2=0;r2<3;r2++){
pose.rotation(r1,r2)=rMat.at<double>(r1,r2);
}
}
tVec=-rMat.t()*tVec;
assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1);
pose.translation(0)=tVec.at<double>(0);
pose.translation(1)=tVec.at<double>(1);
pose.translation(2)=tVec.at<double>(2);
double param_cam[6];
ceres::RotationMatrixToAngleAxis(&pose.rotation(0),¶m_cam[0]);
Eigen::Vector3d translation=pose.rotation*pose.translation;
param_cam[3]=-translation(0);
param_cam[4]=-translation(1);
param_cam[5]=-translation(2);
vector<double> param_invDepth(points3d.size());
ceres::Problem problem;
ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005);
int nInliers=0;
for (int i=0;i<points3d.size();i++) {
cv::Mat point3dMat(3,1,CV_64FC1);
point3dMat.at<double>(0)=points3d[i].x;
point3dMat.at<double>(1)=points3d[i].y;
point3dMat.at<double>(2)=points3d[i].z;
cv::Mat proj=rMat*point3dMat+tVec;
proj/=proj.at<double>(2);
double diffx=proj.at<double>(0)-points2d[i].x;
double diffy=proj.at<double>(1)-points2d[i].y;
double error=std::sqrt(diffx*diffx+diffy*diffy);
param_invDepth[i]=1.0/point3dMat.at<double>(2);
point3dMat/=point3dMat.at<double>(2);
if (error<0.005) {
ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(point3dMat.at<double>(0),
point3dMat.at<double>(1),
(double)points2d[i].x,
(double)points2d[i].y);
problem.AddResidualBlock(cost_function,loss_function,param_cam,¶m_invDepth[i]);
problem.SetParameterBlockConstant(¶m_invDepth[i]);
nInliers++;
}
}
ceres::Solver::Options options;
options.max_num_iterations=5;
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout=false;
ceres::Solver::Summary summary;
ceres::Solve(options,&problem, &summary);
ceres::AngleAxisToRotationMatrix(param_cam,&(keyFrame->mvFarFrames[i].pose.rotation(0)));
keyFrame->mvFarFrames[i].pose.translation=-Eigen::Vector3d(¶m_cam[3]);
keyFrame->mvFarFrames[i].pose.translation=keyFrame->mvFarFrames[i].pose.rotation.transpose()
*keyFrame->mvFarFrames[i].pose.translation;
keyFrame->mvFarFrames[i].pose.setEssentialMatrix();
}
return;
}*/
void LocalBundleAdjustment::refineKeyFrameConnection(KeyFrame* keyFrame){
return;
int num_keyframe=keyFrame->mConnectedKeyFrameMatches.size();
int num_point=keyFrame->mvLocalMapPoints.size();
if (num_keyframe==0||(keyFrame->nextKeyFramePtr!=NULL&&num_keyframe==1)) {
return;
}
printf("%d %d\n",num_keyframe,num_point);
std::vector<double> param_cam(6*num_keyframe);
std::vector<double> param_invDepth(num_point);
std::map<KeyFrame*,int> keyFrameIndex;
int f=0;
for(map<KeyFrame*,Transform>::iterator mit=keyFrame->mConnectedKeyFramePoses.begin(),
mend=keyFrame->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
if (mit->first==keyFrame->nextKeyFramePtr) {//next keyframe's pose is by bundle
continue;
}
keyFrameIndex[mit->first]=f;
ceres::RotationMatrixToAngleAxis(&(mit->second.rotation(0)),¶m_cam[6*f]);
Eigen::Vector3d translation=mit->second.rotation*mit->second.translation;
param_cam[6*f+3]=-translation(0);
param_cam[6*f+4]=-translation(1);
param_cam[6*f+5]=-translation(2);
f++;
}
std::vector<bool> depthFixed(num_point,true);
for (int p=0;p<keyFrame->mvLocalMapPoints.size();p++) {
param_invDepth[p]=keyFrame->mvLocalMapPoints[p].invdepth;
}
ceres::Problem problem;
ceres::LossFunction* loss_function = new ceres::HuberLoss(0.005);
for(map<KeyFrame*,std::vector<int> >::iterator mit=keyFrame->mConnectedKeyFrameMatches.begin(),
mend=keyFrame->mConnectedKeyFrameMatches.end();
mit!=mend;
mit++){
KeyFrame* connectedKeyFrame=mit->first;
if (connectedKeyFrame==keyFrame->nextKeyFramePtr) {
continue;
}
f=keyFrameIndex[connectedKeyFrame];
std::vector<int> &matches=mit->second;
for (int i=0;i<matches.size();i++) {
if (matches[i]<0) {//not matched
continue;
}
if (!keyFrame->mvLocalMapPoints[i].isEstimated) {
assert(0);
std::vector<Eigen::Vector3d> points(2);
std::vector<Eigen::Matrix3d> rotations(2,Eigen::Matrix3d::Identity());
std::vector<Eigen::Vector3d> cameras(2);
Transform transform=keyFrame->mConnectedKeyFramePoses[connectedKeyFrame];
points[0]=keyFrame->mvLocalMapPoints[i].norm;
points[1]=keyFrame->mvLocalMapPoints[matches[i]].norm;
points[1]=transform.rotation.transpose()*points[1];
cameras[0]=Eigen::Vector3d::Zero();
cameras[1]=transform.translation;
//assert(keyFrame->mvLocalMapPoints[i].previousInvDepth==0.0);
Eigen::Vector3d point3D=multiviewTriangulationLinear(points,rotations,cameras);
keyFrame->mvLocalMapPoints[i].invdepth=1.0/point3D(2);
keyFrame->mvLocalMapPoints[i].isEstimated=true;
}
assert(keyFrame->mvLocalMapPoints[i].isEstimated);
Eigen::Vector3d keyFramePoint=keyFrame->mvLocalMapPoints[i].vec;
Eigen::Vector3d framePoint=connectedKeyFrame->mvLocalMapPoints[matches[i]].vec;
ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(keyFramePoint(0),keyFramePoint(1),
framePoint(0),framePoint(1));
problem.AddResidualBlock(cost_function,loss_function,¶m_cam[6*f],¶m_invDepth[i]);
}
}
for (int i=0;i<num_point;i++) {
if (keyFrame->mvLocalMapPoints[i].isEstimated&&keyFrame->mvLocalMapPoints[i].measurementCount>0) {
problem.SetParameterBlockConstant(¶m_invDepth[i]);
}
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
options.minimizer_progress_to_stdout=false;
options.logging_type=ceres::SILENT;
ceres::Solver::Summary summary;
ceres::Solve(options,&problem, &summary);
for (int i=0;i<num_point;i++) {
if (keyFrame->mvLocalMapPoints[i].isEstimated&&keyFrame->mvLocalMapPoints[i].measurementCount<=0) {
keyFrame->mvLocalMapPoints[i].invdepth=param_invDepth[i];
keyFrame->mvLocalMapPoints[i].updateNormalAndDepth();
}
}
for(map<KeyFrame*,Transform>::iterator mit=keyFrame->mConnectedKeyFramePoses.begin(),
mend=keyFrame->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
if (mit->first==keyFrame->nextKeyFramePtr) {
continue;
}
f=keyFrameIndex[mit->first];
ceres::AngleAxisToRotationMatrix(¶m_cam[6*f],&(mit->second.rotation(0)));
mit->second.translation=-Eigen::Vector3d(¶m_cam[6*f+3]);
mit->second.translation=mit->second.rotation.transpose()*mit->second.translation;
mit->second.setEssentialMatrix();
}
return;
}
int LocalBundleAdjustment::refineKeyFrameMatches(KeyFrame *keyFrame1,const KeyFrame *keyFrame2,Transform &transform,
vector<int>& matches12,vector<int>& matches21){
int nMatches=0;
ceres::Problem problem;
ceres::LossFunction* loss_function = new ceres::HuberLoss(projErrorThres);
double param_cam[6];
ceres::RotationMatrixToAngleAxis(&(transform.rotation(0)),¶m_cam[0]);
Eigen::Vector3d translation=transform.rotation*transform.translation;
param_cam[3]=-translation(0);
param_cam[4]=-translation(1);
param_cam[5]=-translation(2);
vector<double> param_invDepth(matches12.size());
std::vector<bool> depthFixed(matches12.size(),false);
for (int i=0;i<matches12.size();i++) {
if (matches12[i]<0) {
continue;
}
if (!keyFrame1->mvLocalMapPoints[i].isEstimated) {
std::vector<Eigen::Vector3d> points(2);
std::vector<Eigen::Matrix3d> rotations(2,Eigen::Matrix3d::Identity());
std::vector<Eigen::Vector3d> cameras(2);
points[0]=keyFrame1->mvLocalMapPoints[i].norm;
points[1]=keyFrame2->mvLocalMapPoints[matches12[i]].norm;
points[1]=transform.rotation.transpose()*points[1];
cameras[0]=Eigen::Vector3d::Zero();
cameras[1]=transform.translation;
Eigen::Vector3d point3D=multiviewTriangulationLinear(points,rotations,cameras);
keyFrame1->mvLocalMapPoints[i].invdepth=1.0/point3D(2);
}else{
depthFixed[i]=true;
}
param_invDepth[i]=keyFrame1->mvLocalMapPoints[i].invdepth;
}
for (int i=0;i<matches12.size();i++) {
if (matches12[i]<0) {
continue;
}else{
nMatches++;
Eigen::Vector3d keyFramePoint=keyFrame1->mvLocalMapPoints[i].vec;
Eigen::Vector3d framePoint=keyFrame2->mvLocalMapPoints[matches12[i]].vec;
ceres::CostFunction* cost_function=SnavelyReprojectionError::Create(keyFramePoint(0),keyFramePoint(1),
framePoint(0),framePoint(1));
problem.AddResidualBlock(cost_function,loss_function,param_cam,¶m_invDepth[i]);
}
}
for (int i=0;i<matches12.size();i++){
if(depthFixed[i]){
problem.SetParameterBlockConstant(¶m_invDepth[i]);
}
}
ceres::Solver::Options options;
options.max_num_iterations=BAIterations;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout=false;
options.logging_type=ceres::SILENT;
ceres::Solver::Summary summary;
ceres::Solve(options,&problem, &summary);
//printf("%d %d before refine match %d\n",keyFrame1->frameId,keyFrame2->frameId,nMatches);
for (int i=0;i<matches12.size();i++) {
if (matches12[i]<0) {
continue;
}
const Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();
Eigen::Vector3d proj=transform.rotation*(point3D-transform.translation);
proj/=proj(2);
double error=(proj-keyFrame2->mvLocalMapPoints[matches12[i]].vec).norm();
if (error>projErrorThres||param_invDepth[i]<0) {
if(!matches21.empty()){
matches21[matches12[i]]=-1;
}
matches12[i]=-1;
nMatches--;
}else if(!depthFixed[i]){
keyFrame1->mvLocalMapPoints[i].isEstimated=true;
keyFrame1->mvLocalMapPoints[i].invdepth=param_invDepth[i];
keyFrame1->mvLocalMapPoints[i].updateNormalAndDepth();
}
}
ceres::AngleAxisToRotationMatrix(param_cam,&(transform.rotation(0)));
transform.translation=-Eigen::Vector3d(¶m_cam[3]);
transform.translation=transform.rotation.transpose()*transform.translation;
transform.setEssentialMatrix();
return nMatches;
}
int PnPEstimator::estimate(KeyFrame* keyFrame1,KeyFrame* keyFrame2,
const vector<int>& matches,Transform& pose){
std::vector<int> indices;
std::vector<cv::Point3f> points3d;
std::vector<cv::Point2f> points2d;
indices.reserve(matches.size());
points3d.reserve(matches.size());
points2d.reserve(matches.size());
//std::cout<<"size "<<matches.size()<<std::endl;
assert(matches.size()>20);
for(int i=0;i<matches.size();i++){
if(matches[i]>=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){
cv::Point3f point3d;
cv::Point2f point2d;
Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();
point3d.x=point3D(0);
point3d.y=point3D(1);
point3d.z=point3D(2);
Eigen::Vector3d normalized3d=keyFrame2->mvLocalMapPoints[matches[i]].norm;
point2d.x=normalized3d(0)/normalized3d(2);
point2d.y=normalized3d(1)/normalized3d(2);
points3d.push_back(point3d);
points2d.push_back(point2d);
indices.push_back(i);
}
}
points3d.shrink_to_fit();
points2d.shrink_to_fit();
indices.shrink_to_fit();
int nInliers=indices.size();
cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1);
cv::Mat rVec,tVec,rMat;
cv::solvePnP(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec);
cv::Rodrigues(rVec,rMat);
for(int r1=0;r1<3;r1++){
for(int r2=0;r2<3;r2++){
pose.rotation(r1,r2)=rMat.at<double>(r1,r2);
}
}
tVec=-rMat.t()*tVec;
assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1);
pose.translation(0)=tVec.at<double>(0);
pose.translation(1)=tVec.at<double>(1);
pose.translation(2)=tVec.at<double>(2);
return nInliers;
}
int PnPEstimator::estimate(KeyFrame* keyFrame1,KeyFrame* keyFrame2,
vector<int>& matches12,vector<int>& matches21,Transform& pose){
std::vector<int> indices;
std::vector<cv::Point3f> points3d;
std::vector<cv::Point2f> points2d;
indices.reserve(matches12.size());
points3d.reserve(matches12.size());
points2d.reserve(matches12.size());
//std::cout<<"size "<<matches.size()<<std::endl;
for(int i=0;i<matches12.size();i++){
if(matches12[i]>=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){
cv::Point3f point3d;
cv::Point2f point2d;
Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();
point3d.x=point3D(0);
point3d.y=point3D(1);
point3d.z=point3D(2);
Eigen::Vector3d normalized3d=keyFrame2->mvLocalMapPoints[matches12[i]].norm;
point2d.x=normalized3d(0)/normalized3d(2);
point2d.y=normalized3d(1)/normalized3d(2);
points3d.push_back(point3d);
points2d.push_back(point2d);
indices.push_back(i);
}
}
points3d.shrink_to_fit();
points2d.shrink_to_fit();
indices.shrink_to_fit();
int nInliers=indices.size();
cv::Mat cameraMatrix=cv::Mat::eye(3,3,CV_64FC1);
cv::Mat rVec,tVec,rMat;
std::vector<uchar> status(points3d.size());
cv::solvePnPRansac(points3d,points2d,cameraMatrix,cv::Mat(),rVec,tVec,false,1000,threshold,prob);
cv::Rodrigues(rVec,rMat);
for(int r1=0;r1<3;r1++){
for(int r2=0;r2<3;r2++){
pose.rotation(r1,r2)=rMat.at<double>(r1,r2);
}
}
tVec=-rMat.t()*tVec;
assert(rMat.type()==CV_64FC1&&tVec.type()==CV_64FC1);
pose.translation(0)=tVec.at<double>(0);
pose.translation(1)=tVec.at<double>(1);
pose.translation(2)=tVec.at<double>(2);
for (int i=0;i<matches12.size();i++) {
if(matches12[i]>=0&&keyFrame1->mvLocalMapPoints[i].isEstimated==true){
Eigen::Vector3d proj=keyFrame1->mvLocalMapPoints[i].getPosition();
proj=pose.rotation*(proj-pose.translation);
proj/=proj(2);
Eigen::Vector3d vec=keyFrame2->mvLocalMapPoints[matches12[i]].vec;
Eigen::Vector3d error=proj-vec;
if (error.norm()>threshold) {
matches21[matches12[i]]=-1;
matches12[i]=-1;
nInliers--;
}
}
}
return nInliers;
}
}
================================================
FILE: GSLAM/Estimation.h
================================================
//
// Optimization.h
// GSLAM
//
// Created by ctang on 9/4/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include "KLT.h"
#include "KeyFrame.h"
namespace GSLAM {
class Undistortion{
};
class RelativeOutlierRejection{
public:
Eigen::Matrix3d K;
Eigen::Matrix3d rotation;
float theshold;
float prob;
double minViewAngle;
double medViewAngle;
vector<bool>* isValid;
int relativeRansac(const KeyFrame* keyFrame,
const KLT_FeatureList featureList);
};
class RelativePoseEstimation{
public:
bool rotationIsKnown;
Eigen::Matrix3d rotation;
Eigen::Vector3d translation;
double medViewAngle;
double minViewAngle;
vector<bool>* isValid;
int estimateRelativePose(const KeyFrame* keyFrame,
const KLT_FeatureList featureList,
std::vector<Eigen::Vector3d*> &pVectors);
void unitTest();
private:
};
class LocalFactorization{
public:
void process(KeyFrame *keyFrame);
void iterativeRefine(KeyFrame *keyFrame);
};
class LocalBundleAdjustment{
public:
double projErrorThres;
double viewAngleThres;
void refinePoints(KeyFrame* keyFrame);
void triangulate2(KeyFrame* keyFrame);
void bundleAdjust(KeyFrame *keyFrame,bool cameraFixed=false);
void triangulate(KeyFrame *keyFrame);
void refineKeyFrameConnection(KeyFrame *keyFrame);
int refineKeyFrameMatches(KeyFrame* keyFrame1,const KeyFrame* keyFrame2,Transform& transform,
std::vector<int>& matches12,std::vector<int>& matches21);
//void estimateFarFrames(KeyFrame* keyFrame);
int BAIterations;
};
class PnPEstimator{
public:
int minCount;
double threshold;
double prob;
int estimate(KeyFrame *keyFrame1,KeyFrame *keyFrame2,
const vector<int>& matches,Transform& pose);
int estimate(KeyFrame *keyFrame1,KeyFrame *keyFrame2,
vector<int>& matches12,vector<int>& matches21,Transform& pose);
};
}
================================================
FILE: GSLAM/Frame.cpp
================================================
//
// Frame.cpp
// GSLAM
//
// Created by ctang on 9/4/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include "Frame.h"
#include "opencv2/imgproc/imgproc.hpp"
namespace GSLAM{
long unsigned int Frame::nNextId=0;
bool Frame::mbInitialComputations=true;
float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy;
float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY;
float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv;
Frame::Frame(cv::Mat &im, const double &timeStamp,
ORBextractor* extractor, ORBVocabulary* voc,
cv::Mat &K, cv::Mat &distCoef)
:mpORBvocabulary(voc),mpORBextractor(extractor),
mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()){
keyFramePtr=NULL;
//im.copyTo(debugImage);
#ifdef DEBUG
//im.copyTo(debugImage);
#endif
// Exctract ORB
(*mpORBextractor)(im,cv::Mat(),mvKeys,mDescriptors);
N = mvKeys.size();
if(mvKeys.empty())
return;
mvpLocalMapPoints = vector<LocalMapPoint*>(N,static_cast<LocalMapPoint*>(NULL));
UndistortKeyPoints();
// This is done for the first created Frame
if(mbInitialComputations){
ComputeImageBounds(im);
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)
/static_cast<float>(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)
/static_cast<float>(mnMaxY-mnMinY);
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
mbInitialComputations=false;
}
mnId=nNextId++;
//Scale Levels Info
mnScaleLevels = mpORBextractor->GetLevels();
mfScaleFactor = mpORBextractor->GetScaleFactor();
mfLogScaleFactor = std::log(mfScaleFactor);
mvScaleFactors.resize(mnScaleLevels);
mvLevelSigma2.resize(mnScaleLevels);
mvScaleFactors[0]=1.0f;
mvLevelSigma2[0]=1.0f;
for(int i=1; i<mnScaleLevels; i++){
mvScaleFactors[i]=mvScaleFactors[i-1]*mfScaleFactor;
mvLevelSigma2[i]=mvScaleFactors[i]*mvScaleFactors[i];
}
mvInvLevelSigma2.resize(mvLevelSigma2.size());
for(int i=0; i<mnScaleLevels; i++)
mvInvLevelSigma2[i]=1/mvLevelSigma2[i];
// Assign Features to Grid Cells
int nReserve = 0.5*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
mGrid[i][j].reserve(nReserve);
for(size_t i=0;i<mvKeysUn.size();i++){
cv::KeyPoint &kp = mvKeysUn[i];
int nGridPosX, nGridPosY;
if(PosInGrid(kp,nGridPosX,nGridPosY)){
mGrid[nGridPosX][nGridPosY].push_back(i);
}
}
mvbOutlier = vector<bool>(N,false);
ComputeBoW();
}
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY){
posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);
//Keypoint's coordinates are undistorted, which could cause to go out of the image
if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
return false;
return true;
}
void Frame::ComputeImageBounds(const cv::Mat &im)
{
if(mDistCoef.at<float>(0)!=0.0){
cv::Mat mat(4,2,CV_32F);
mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;
mat.at<float>(1,0)=im.cols; mat.at<float>(1,1)=0.0;
mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=im.rows;
mat.at<float>(3,0)=im.cols; mat.at<float>(3,1)=im.rows;
// Undistort corners
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);
mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));
mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));
mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));
mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));
}else{
mnMinX = 0.0f;
mnMaxX = im.cols;
mnMinY = 0.0f;
mnMaxY = im.rows;
}
}
void Frame::UndistortKeyPoints()
{
if(mDistCoef.at<float>(0)==0.0){
mvKeysUn=mvKeys;
return;
}
// Fill matrix with points
cv::Mat mat(N,2,CV_32F);
for(int i=0; i<N; i++)
{
mat.at<float>(i,0)=mvKeys[i].pt.x;
mat.at<float>(i,1)=mvKeys[i].pt.y;
}
// Undistort points
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);
// Fill undistorted keypoint vector
mvKeysUn.resize(N);
for(int i=0; i<N; i++)
{
cv::KeyPoint kp = mvKeys[i];
kp.pt.x=mat.at<float>(i,0);
kp.pt.y=mat.at<float>(i,1);
mvKeysUn[i]=kp;
}
}
void Frame::ComputeBoW(){
if(mBowVec.empty()){
vector<cv::Mat> vCurrentDesc = toDescriptorVector(mDescriptors);
mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
}
}
std::vector<cv::Mat> Frame::toDescriptorVector(const cv::Mat &Descriptors){
std::vector<cv::Mat> vDesc;
vDesc.reserve(Descriptors.rows);
for (int j=0;j<Descriptors.rows;j++)
vDesc.push_back(Descriptors.row(j));
return vDesc;
}
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r,
const int minLevel, const int maxLevel,vector<float> &distances) const
{
vector<size_t> vIndices;
vIndices.reserve(N);
distances.reserve(N);
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
const vector<size_t> vCell = mGrid[ix][iy];
if(vCell.empty())
continue;
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
if(bCheckLevels){
if(kpUn.octave<minLevel)
continue;
if(maxLevel>=0)
if(kpUn.octave>maxLevel)
continue;
}
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
if(fabs(distx)<r && fabs(disty)<r){
vIndices.push_back(vCell[j]);
distances.push_back(distx*distx+disty*disty);
}
}
}
}
vIndices.shrink_to_fit();
distances.shrink_to_fit();
return vIndices;
}
}
================================================
FILE: GSLAM/Frame.h
================================================
/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/GSLAM>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef FRAME_H
#define FRAME_H
#include<vector>
#include "Settings.h"
#include "Transform.h"
#include "KeyFrame.h"
#include "./DBoW2/BowVector.h"
#include "./DBoW2/FeatureVector.h"
#include "ORBVocabulary.h"
#include "ORBextractor.h"
#include <opencv2/core/core.hpp>
namespace GSLAM
{
#define FRAME_GRID_ROWS 54
#define FRAME_GRID_COLS 96
class LocalMapPoint;
class KeyFrame;
class Frame
{
public:
Frame(cv::Mat &im, const double &timeStamp, ORBextractor* extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef);
// Copy constructor.
Frame(const Frame &frame);
public:
int mnId;
KeyFrame* keyFramePtr;
// Vocabulary used for relocalization.
ORBVocabulary* mpORBvocabulary;
// Feature extractor. The right is used only in the stereo case.
ORBextractor* mpORBextractor;
DBoW2::BowVector mBowVec;
DBoW2::FeatureVector mFeatVec;
int N;
int NCoarse;
cv::Mat mDescriptors;
std::vector<cv::KeyPoint> mvKeys;
std::vector<cv::KeyPoint> mvKeysUn;
std::vector<bool> mvbOutlier;
std::vector<LocalMapPoint*> mvpLocalMapPoints;
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
// Frame timestamp.
double mTimeStamp;
//static members
static float mnMinX;
static float mnMaxX;
static float mnMinY;
static float mnMaxY;
static bool mbInitialComputations;
static long unsigned int nNextId;
static float mfGridElementWidthInv;
static float mfGridElementHeightInv;
cv::Mat mK;
static float fx;
static float fy;
static float cx;
static float cy;
static float invfx;
static float invfy;
cv::Mat mDistCoef;
float mfOriginalScaleFacotr;
// Scale pyramid info.
int mnScaleLevels;
float mfScaleFactor;
float mfLogScaleFactor;
vector<float> mvScaleFactors;
vector<float> mvInvScaleFactors;
vector<float> mvLevelSigma2;
vector<float> mvInvLevelSigma2;
bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
void ComputeImageBounds(const cv::Mat &im);
void UndistortKeyPoints();
void ComputeBoW();
vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r,
const int minLevel, const int maxLevel,vector<float> &distances) const;
#ifdef DEBUG
cv::Mat debugImage;
#endif
};
}
#endif
================================================
FILE: GSLAM/Geometry.cpp
================================================
//
// Geometry.cpp
// GSLAM
//
// Created by ctang on 9/17/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include <stdio.h>
#include "Geometry.h"
#include "ceres/ceres.h"
Eigen::Vector3d multiviewTriangulationLinear(const std::vector<Eigen::Vector3d> &points,
const std::vector<Eigen::Matrix3d> &rotations,
const std::vector<Eigen::Vector3d> &cameras){
Eigen::Vector3d point3D;
int ncamera=points.size();
Eigen::MatrixX4d A=Eigen::MatrixX4d::Zero(3*ncamera,4);
Eigen::MatrixX4d pLastRow=Eigen::MatrixX4d::Zero(ncamera,4);
for(int i=0;i<ncamera;i++){
Eigen::Matrix3d crossMat;
Eigen::MatrixX4d P=Eigen::MatrixX4d(3,4);
setCrossMatrix(crossMat,points[i]);
P.block<3,3>(0,0)=rotations[i];
P.col(3)=-rotations[i]*cameras[i];
A.block<3,4>(3*i,0)=crossMat*P;
pLastRow.row(i)=P.row(2);
}
Eigen::Matrix4d AtA=A.transpose()*A;
Eigen::JacobiSVD<Eigen::Matrix4d> svd(AtA,Eigen::ComputeFullV);
Eigen::Vector4d solution=svd.matrixV().col(3);
point3D(0)=solution(0)/solution(3);
point3D(1)=solution(1)/solution(3);
point3D(2)=solution(2)/solution(3);
Eigen::VectorXd product=pLastRow*solution;
int positiveCount=0;
for(int i=0;i<ncamera;i++){
positiveCount+=(product(i)>0);
}
if(positiveCount<ncamera/2){
solution=-solution;
}
point3D(0)=solution(0)/solution(3);
point3D(1)=solution(1)/solution(3);
point3D(2)=solution(2)/solution(3);
return point3D;
}
/*void multiviewTriangulationNonliear(double& depth,
const std::vector<Eigen::Vector3d>& points,
const std::vector<Eigen::Matrix3d>& rotations,
const std::vector<Eigen::Vector3d>& cameras){
}*/
double multiviewTriangulationDepth(const std::vector<Eigen::Vector3d> &points,
const std::vector<Eigen::Matrix3d> &rotations,
const std::vector<Eigen::Vector3d> &cameras){
int ncameras=points.size()-1;
Eigen::VectorXd A=Eigen::VectorXd(3*ncameras);
Eigen::VectorXd B=Eigen::VectorXd(3*ncameras);
for (int i=1;i<=ncameras;i++) {
Eigen::Matrix3d crossMat;
Eigen::Vector3d rotated=rotations[i].transpose()*points[i];
setCrossMatrix(crossMat,rotated);
rotated=crossMat*points[0];
A.block<3,1>(3*(i-1),0)=rotated;
B.block<3,1>(3*(i-1),0)=crossMat*cameras[i];
}
double a=A.dot(A);
double b=A.dot(B);
return std::abs(b/a);
}
/*void multiviewTriangulationNonliear(double& depth,
const std::vector<Eigen::Vector3d>& points,
const std::vector<Eigen::Matrix3d>& rotations,
const std::vector<Eigen::Vector3d>& cameras){
}*/
inline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta,
const double sintheta,
const double wx,
const double wy,
const double wz){
Eigen::Matrix3d R;
R(0,0) = costheta + wx*wx*(1 - costheta);
R(1,0) = wz*sintheta + wx*wy*(1 - costheta);
R(2,0) = -wy*sintheta + wx*wz*(1 - costheta);
R(0,1) = wx*wy*(1 - costheta) - wz*sintheta;
R(1,1) = costheta + wy*wy*(1 - costheta);
R(2,1) = wx*sintheta + wy*wz*(1 - costheta);
R(0,2) = wy*sintheta + wx*wz*(1 - costheta);
R(1,2) = -wx*sintheta + wy*wz*(1 - costheta);
R(2,2) = costheta + wz*wz*(1 - costheta);
return R;
}
Eigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eigen::Vector3d &v2){
double cos12=v1.dot(v2)/(v1.norm()*v2.norm());
double sin12=sqrt(1-cos12*cos12);
Eigen::Vector3d axis =v1.cross(v2);
axis.normalize();
Eigen::Matrix3d R=AngleAxis2RotationMatrix(cos12,sin12,axis(0),axis(1),axis(2));
return R;
}
================================================
FILE: GSLAM/Geometry.h
================================================
//
// Geometry.hpp
// GSLAM
//
// Created by ctang on 9/17/16.
// Copyright © 2016 ctang. All rights reserved.
//
#ifndef Geometry_h
#define Geometry_h
#include "Eigen/Dense"
#include <vector>
inline void setCrossMatrix(Eigen::Matrix3d &crossMat,const Eigen::Vector3d& vector){
crossMat(0,0)=crossMat(1,1)=crossMat(2,2)=0.0;
crossMat(0,1)= vector(2);
crossMat(0,2)= -vector(1);
crossMat(1,0)= -vector(2);
crossMat(1,2)= vector(0);
crossMat(2,0)= vector(1);
crossMat(2,1)= -vector(0);
}
Eigen::Vector3d multiviewTriangulationLinear(const std::vector<Eigen::Vector3d> &points,
const std::vector<Eigen::Matrix3d> &rotations,
const std::vector<Eigen::Vector3d> &cameras);
double multiviewTriangulationDepth(const std::vector<Eigen::Vector3d> &points,
const std::vector<Eigen::Matrix3d> &rotations,
const std::vector<Eigen::Vector3d> &cameras);
void multiviewTriangulationNonlinear(double& depth,
const std::vector<Eigen::Vector3d> &points,
const std::vector<Eigen::Matrix3d> &rotations,
const std::vector<Eigen::Vector3d> &cameras);
inline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta,
const double sintheta,
const double wx,
const double wy,
const double wz);
Eigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eigen::Vector3d &v2);
#endif /* Geometry_h */
================================================
FILE: GSLAM/GlobalReconstruction.cpp
================================================
//
// GlobalReconstruction.cpp
// GSLAM
//
// Created by ctang on 9/28/16.
// Copyright © 2016 ctang. All rights reserved.
//
//#include "opencv2/viz.hpp"
#include "GlobalReconstruction.h"
#include "KeyFrame.h"
#include "Drawer.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "ceres/ceres.h"
#include "theia/sfm/global_pose_estimation/robust_rotation_estimator.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include <vector>
const bool disable_loop=true;
namespace GSLAM {
void GlobalReconstruction::getScaleConstraint(KeyFrame* keyFrame1,
vector<ScaleConstraint>& scaleConstraints){
for(map<KeyFrame*,vector<int> >::iterator mit=keyFrame1->mConnectedKeyFrameMatches.begin(),
mend=keyFrame1->mConnectedKeyFrameMatches.end();
mit!=mend;
mit++){
KeyFrame* keyFrame2=mit->first;
if (keyFrame2->mvLocalFrames.empty()) {
continue;
}
Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];
ScaleConstraint constraint;
constraint.keyFrameIndex1=keyFrame1->mnId;
constraint.keyFrameIndex2=keyFrame2->mnId;
vector<double> scales;
vector<int>& matches=mit->second;
for (int i=0;i<matches.size();i++) {
if (matches[i]>=0) {
assert(keyFrame1->mvLocalMapPoints[i].isEstimated);
if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated){
continue;
}
Eigen::Vector3d distance1=keyFrame1->mvLocalMapPoints[i].getPosition()-pose.translation;
Eigen::Vector3d distance2=keyFrame2->mvLocalMapPoints[matches[i]].getPosition();
/*printf("%d %d %x %x\n",keyFrame1->outId,keyFrame2->outId,
keyFrame1->mvLocalMapPoints[i].gMP,keyFrame2->mvLocalMapPoints[matches[i]].gMP);
for(map<KeyFrame*,int>::iterator mit=keyFrame1->mvLocalMapPoints[i].gMP->measurements.begin(),
mend=keyFrame1->mvLocalMapPoints[i].gMP->measurements.end();
mit!=mend; mit++){
printf("%d %d\n",mit->first->outId,mit->second);
}
for(map<KeyFrame*,int>::iterator mit=keyFrame2->mvLocalMapPoints[matches[i]].gMP->measurements.begin(),
mend=keyFrame2->mvLocalMapPoints[matches[i]].gMP->measurements.end();
mit!=mend; mit++){
printf("%d %d\n",mit->first->outId,mit->second);
}*/
//assert(keyFrame1->mvLocalMapPoints[i].gMP==keyFrame2->mvLocalMapPoints[matches[i]].gMP);
double relativeScale=distance2.norm()/distance1.norm();
scales.push_back(relativeScale);
}
}
if (scales.size()>=scaleThreshold) {
sort(scales.begin(),scales.end());
constraint.value12=log(scales[scales.size()/2]);
}else{
continue;
}
constraint.weight=1.0;
scaleConstraints.push_back(constraint);
}
}
void GlobalReconstruction::getRotationConstraint(KeyFrame* keyFrame1,
vector<RotationConstraint>& rotationConstraints){
for(map<KeyFrame*,Transform>::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),
mend=keyFrame1->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
KeyFrame* keyFrame2=mit->first;
Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];
RotationConstraint constraint;
constraint.keyFrameIndex1=keyFrame1->mnId;
constraint.keyFrameIndex2=keyFrame2->mnId;
constraint.rotation12=pose.rotation;
if(pose.scale==-1&&(keyFrame1->frameId<70||keyFrame2->frameId<70)){
continue;
}
//printf("add %d %d\n",constraint.keyFrameIndex1,constraint.keyFrameIndex2);
rotationConstraints.push_back(constraint);
}
}
void GlobalReconstruction::getTranslationConstraint(KeyFrame* keyFrame1,
vector<TranslationConstraint>& translationConstraints){
for(map<KeyFrame*,Transform>::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),
mend=keyFrame1->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
KeyFrame* keyFrame2=mit->first;
Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];
TranslationConstraint constraint;
constraint.keyFrameIndex1=keyFrame1->mnId;
constraint.keyFrameIndex2=keyFrame2->mnId;
constraint.rotation1=keyFrame1->pose.rotation;
constraint.translation12=pose.translation/keyFrame1->scale;
if(pose.scale==-1&&(keyFrame1->frameId<70||keyFrame2->frameId<70)){
continue;
}
translationConstraints.push_back(constraint);
}
}
void GlobalReconstruction::getSIM3Constraint(KeyFrame* keyFrame1,vector<SIM3Constraint>& sim3Constraints){
for(map<KeyFrame*,Transform>::iterator mit=keyFrame1->mConnectedKeyFramePoses.begin(),
mend=keyFrame1->mConnectedKeyFramePoses.end();
mit!=mend;
mit++){
KeyFrame* keyFrame2=mit->first;
Transform pose=keyFrame1->mConnectedKeyFramePoses[keyFrame2];
SIM3Constraint constraint;
constraint.keyFrameIndex1=keyFrame1->mnId;
constraint.keyFrameIndex2=keyFrame2->mnId;
constraint.rotation12=pose.rotation;
constraint.translation12=pose.translation;
constraint.weight=1.0;
vector<int>& matches=keyFrame1->mConnectedKeyFrameMatches[keyFrame2];
vector<double> scales;
for (int i=0;i<matches.size();i++) {
if (matches[i]>=0) {
assert(keyFrame1->mvLocalMapPoints[i].isEstimated);
if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated){
continue;
}
Eigen::Vector3d distance1=keyFrame1->mvLocalMapPoints[i].getPosition()-pose.translation;
Eigen::Vector3d distance2=keyFrame2->mvLocalMapPoints[matches[i]].getPosition();
double relativeScale=distance2.norm()/distance1.norm();
scales.push_back(relativeScale);
}
}
if(scales.empty()){
continue;
}
sort(scales.begin(),scales.end());
constraint.scale12=scales[scales.size()/2];
sim3Constraints.push_back(constraint);
}
}
void GlobalReconstruction::estimateScale(){
vector<int> scaleIndex(keyFrames.size(),-1);
vector<ScaleConstraint> scaleConstraints;
for (int i=0;i<keyFrames.size();i++) {
assert(keyFrames[i]->mnId==i);
getScaleConstraint(keyFrames[i],scaleConstraints);
}
//printf("%d keyframe size\n",keyFrames.size());
int nScales=0;
for (int i=0;i<scaleConstraints.size();i++) {
if (scaleIndex[scaleConstraints[i].keyFrameIndex1]==-1) {
scaleIndex[scaleConstraints[i].keyFrameIndex1]=nScales;
nScales++;
}
if (scaleIndex[scaleConstraints[i].keyFrameIndex2]==-1) {
scaleIndex[scaleConstraints[i].keyFrameIndex2]=nScales;
nScales++;
}
//printf("%d %d\n",scaleIndex[scaleConstraints[i].keyFrameIndex1],scaleIndex[scaleConstraints[i].keyFrameIndex2]);
scaleConstraints[i].variableIndex1=scaleIndex[scaleConstraints[i].keyFrameIndex1];
scaleConstraints[i].variableIndex2=scaleIndex[scaleConstraints[i].keyFrameIndex2];
}
//extrac constraint for first keyframe
ScaleConstraint constraint;
constraint.keyFrameIndex1=-1;
constraint.keyFrameIndex2=0;
constraint.variableIndex1=-1;
constraint.variableIndex2=0;
constraint.value1=0.0;
constraint.value12=0.0;
constraint.weight=1.0;
scaleConstraints.push_back(constraint);
vector<double> newScales;
newScales.resize(nScales);
for(int i=0;i<keyFrames.size();i++){
if (scaleIndex[i]!=-1) {
newScales[scaleIndex[i]]=keyFrames[i]->logScale;
}
}
/*for (int i=0;i<scaleConstraints.size();i++) {
cout<<scaleConstraints[i].keyFrameIndex1<<' '
<<scaleConstraints[i].keyFrameIndex2<<' '
<<scaleConstraints[i].variableIndex1<<' '
<<scaleConstraints[i].variableIndex2<<' '
<<scaleConstraints[i].value12<<endl;
}
for (int i=0;i<newScales.size();i++) {
cout<<i<<' '<<newScales[i]<<endl;
}*/
globalScaleEstimation.maxIterations=10000;
globalScaleEstimation.solve(scaleConstraints,newScales);
for (int i=1;i<newScales.size();i++) {
newScales[i]-=newScales[0];
}
newScales[0]=0.0;
for (int i=0;i<keyFrames.size();i++) {
keyFrames[i]->logScale=newScales[scaleIndex[i]];
keyFrames[i]->scale=exp(keyFrames[i]->logScale);
printf("%f\n",keyFrames[i]->scale);
}
static ofstream record("/Users/chaos/Desktop/debug/scales_error.txt");
for (int i=0;i<scaleConstraints.size();i++) {
if (scaleConstraints[i].keyFrameIndex1==-1) {
continue;
}
double diff=keyFrames[scaleConstraints[i].keyFrameIndex2]->logScale-keyFrames[scaleConstraints[i].keyFrameIndex1]->logScale;
diff=abs(diff-scaleConstraints[i].value12);
int id1=keyFrames[scaleConstraints[i].keyFrameIndex1]->outId;
int id2=keyFrames[scaleConstraints[i].keyFrameIndex2]->outId;
//printf("%f %d %d\n",diff,id1,id2);
record<<id1<<' '
<<id2<<' '
<<diff<<endl;
}
//getchar();
}
void GlobalReconstruction::estimateRotation(vector<int> &rotationIndex){
std::vector<RotationConstraint> rotationConstraints(0);
std::vector<Eigen::Matrix3d> newRotations;
int nRotations=0;
rotationIndex.resize(keyFrames.size(),-1);
for (int k=0;k<keyFrames.size();k++) {
getRotationConstraint(keyFrames[k],rotationConstraints);
for (int i=0;i<rotationConstraints.size();i++) {
if (rotationIndex[rotationConstraints[i].keyFrameIndex1]==-1) {
rotationIndex[rotationConstraints[i].keyFrameIndex1]=nRotations;
nRotations++;
newRotations.push_back(keyFrames[rotationConstraints[i].keyFrameIndex1]->pose.rotation);
}
if (rotationIndex[rotationConstraints[i].keyFrameIndex2]==-1) {
rotationIndex[rotationConstraints[i].keyFrameIndex2]=nRotations;
nRotations++;
newRotations.push_back(keyFrames[rotationConstraints[i].keyFrameIndex2]->pose.rotation);
}
rotationConstraints[i].variableIndex1=rotationIndex[rotationConstraints[i].keyFrameIndex1];
rotationConstraints[i].variableIndex2=rotationIndex[rotationConstraints[i].keyFrameIndex2];
}
}
std::unordered_map<theia::ViewIdPair,theia::TwoViewInfo> view_pairs;
std::unordered_map<theia::ViewId,Eigen::Vector3d> orientations;
theia::RobustRotationEstimator::Options options;
options.max_num_irls_iterations=100;
options.max_num_l1_iterations=10;
theia::RobustRotationEstimator rotation_estimator(options);
for (int i=0;i<rotationConstraints.size();i++) {
theia::ViewIdPair viewPair;
viewPair.first=rotationConstraints[i].variableIndex1;
viewPair.second=rotationConstraints[i].variableIndex2;
Eigen::Vector3d angle;
ceres::RotationMatrixToAngleAxis(rotationConstraints[i].rotation12.data(),angle.data());
view_pairs[viewPair].rotation_2=angle;
}
newRotations.resize(keyFrames.size());
newRotations[0]=Eigen::Matrix3d::Identity();
for (int i=1;i<keyFrames.size();i++) {
newRotations[i]=keyFrames[i-1]->mvLocalFrames.back().pose.rotation*newRotations[i-1];
}
for (int i=0;i<keyFrames.size();i++) {
Eigen::Vector3d angle;
ceres::RotationMatrixToAngleAxis(newRotations[i].data(),angle.data());
orientations[i]=angle;
}
std::cout<<"theia rotation start"<<std::endl;
rotation_estimator.EstimateRotations(view_pairs,&orientations);
std::cout<<"theia rotation finished"<<std::endl;
for (int i=0;i<newRotations.size();i++) {
ceres::AngleAxisToRotationMatrix(orientations[i].data(),newRotations[i].data());
std::cout<<orientations[i].transpose()<<std::endl;
}
for (int i=1;i<newRotations.size();i++) {
newRotations[i]=newRotations[i]*newRotations[0].transpose();
}
newRotations[0]=Eigen::Matrix3d::Identity();
for (int k=0;k<keyFrames.size();k++) {
keyFrames[k]->pose.rotation=newRotations[rotationIndex[k]];
}
std::cout<<"rotation end"<<std::endl;
}
void GlobalReconstruction::estimateRotationRobust(const vector<int> &rotationIndex){
vector<RotationConstraint> rotationConstraints(0);
vector<Eigen::Matrix3d> newRotations(keyFrames.size());
for (int k=0;k<keyFrames.size();k++) {
getRotationConstraint(keyFrames[k],rotationConstraints);
for (int i=0;i<rotationConstraints.size();i++) {
rotationConstraints[i].variableIndex1=rotationIndex[rotationConstraints[i].keyFrameIndex1];
rotationConstraints[i].variableIndex2=rotationIndex[rotationConstraints[i].keyFrameIndex2];
}
}
for (int i=0;i<newRotations.size();i++) {
newRotations[i]=keyFrames[i]->pose.rotation;
}
RotationConstraint constraint;
constraint.keyFrameIndex1=-1;
constraint.keyFrameIndex2=0;
constraint.variableIndex1=-1;
constraint.variableIndex2=0;
constraint.rotation1=Eigen::Matrix3d::Identity();
constraint.rotation12=Eigen::Matrix3d::Identity();
constraint.weight=1.0;
rotationConstraints.push_back(constraint);
globalRotationEstimation.maxOuterIterations=1000;
globalRotationEstimation.maxInnerIterations=20;
globalRotationEstimation.solve(rotationConstraints,newRotations);
for (int i=1;i<newRotations.size();i++) {
newRotations[i]=newRotations[i]*newRotations[0].transpose();
}
newRotations[0]=Eigen::Matrix3d::Identity();
for (int k=0;k<keyFrames.size();k++) {
keyFrames[k]->pose.rotation=newRotations[rotationIndex[k]];
}
}
void GlobalReconstruction::estimateTranslation(const vector<int> &translationIndex){
vector<TranslationConstraint> translationConstraints;
for (int k=0;k<keyFrames.size();k++) {
getTranslationConstraint(keyFrames[k],translationConstraints);
}
for (int i=0;i<translationConstraints.size();i++) {
translationConstraints[i].variableIndex1=translationIndex[translationConstraints[i].keyFrameIndex1];
translationConstraints[i].variableIndex2=translationIndex[translationConstraints[i].keyFrameIndex2];
}
TranslationConstraint constraint;
constraint.keyFrameIndex1=-1;
constraint.keyFrameIndex2=0;
constraint.variableIndex1=-1;
constraint.variableIndex2=0;
constraint.rotation1=Eigen::Matrix3d::Identity();
constraint.translation1=Eigen::Vector3d::Zero();
constraint.translation12=Eigen::Vector3d::Zero();
constraint.weight=1.0;
translationConstraints.push_back(constraint);
for (int i=0;i<translationConstraints.size();i++) {
translationConstraints[i].translation12=translationConstraints[i].rotation1.transpose()
*translationConstraints[i].translation12;
/*cout<<translationConstraints[i].variableIndex1<<' '
<<translationConstraints[i].variableIndex2<<endl
<<translationConstraints[i].translation12<<endl<<translationConstraints[i].rotation1<<endl;*/
}
vector<Eigen::Vector3d> newTranslations(keyFrames.size(),Eigen::Vector3d::Zero());
globalTranslationEstimation.maxIterations=10000;
globalTranslationEstimation.solve(translationConstraints,newTranslations);
for (int i=1;i<newTranslations.size();i++) {
newTranslations[i]-=newTranslations[0];
}
newTranslations[0]=Eigen::Vector3d::Zero();
for (int k=0;k<keyFrames.size();k++) {
keyFrames[k]->pose.translation=newTranslations[translationIndex[k]];
cout<<keyFrames[k]->pose.translation.transpose()<<endl;
}
static ofstream record("/Users/chaos/Desktop/debug/trans_error.txt");
for (int i=0;i<translationConstraints.size();i++) {
if (translationConstraints[i].keyFrameIndex1==-1) {
continue;
}
Eigen::Vector3d differror=translationConstraints[i].translation12-(keyFrames[translationConstraints[i].keyFrameIndex2]->pose.translation-keyFrames[translationConstraints[i].keyFrameIndex1]->pose.translation);
double diff=differror.norm();
int id1=keyFrames[translationConstraints[i].keyFrameIndex1]->outId;
int id2=keyFrames[translationConstraints[i].keyFrameIndex2]->outId;
record<<id1<<' '
<<id2<<' '
<<diff<<endl;
}
}
struct GlobalError2{
GlobalError2(const double _tracked_x,const double _tracked_y):
tracked_x(_tracked_x), tracked_y(_tracked_y){
}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
T transformed[3];
ceres::AngleAxisRotatePoint(camera,point,transformed);
transformed[0]+=camera[3];
transformed[1]+=camera[4];
transformed[2]+=camera[5];
T predicted_x=transformed[0]/transformed[2];
T predicted_y=transformed[1]/transformed[2];
residuals[0]=predicted_x-(T)tracked_x;
residuals[1]=predicted_y-(T)tracked_y;
residuals[0]=residuals[0];
residuals[1]=residuals[1];
return true;
}
static ceres::CostFunction* Create(const double tracked_x,
const double tracked_y) {
return (new ceres::AutoDiffCostFunction<GlobalError2,2,6,3>(new GlobalError2(tracked_x,tracked_y)));
};
double tracked_x;
double tracked_y;
};
void GlobalReconstruction::globalRefine(){
std::vector<double> points;
std::vector<double> cameras;
std::vector<std::pair<int,int>> pairCameraPoint;
int nPoints=0;
cameras.resize(6*keyFrames.size());
for (int i=0;i<keyFrames.size();i++) {
KeyFrame* keyFrame=keyFrames[i];
Eigen::Vector3d translation=-keyFrame->pose.rotation*keyFrame->pose.translation;
ceres::RotationMatrixToAngleAxis(keyFrame->pose.rotation.data(),
&cameras[6*i]);
cameras[6*i+3]=translation(0);
cameras[6*i+4]=translation(1);
cameras[6*i+5]=translation(2);
nPoints+=keyFrame->mvLocalMapPoints.size();
}
points.resize(3*nPoints);
nPoints=0;
ceres::Problem problem;
ceres::LossFunction* loss_function = new ceres::HuberLoss(0.003);
for (int k=0;k<keyFrames.size();k++) {
KeyFrame* keyFrame1=keyFrames[k];
assert(keyFrame1->mnId==k);
for (int i=0;i<keyFrame1->mvLocalMapPoints.size();i++) {
if (keyFrame1->mvLocalMapPoints[i].isEstimated) {
Eigen::Vector3d point3D=keyFrame1->mvLocalMapPoints[i].getPosition();
point3D/=keyFrame1->scale;
point3D=keyFrame1->pose.rotation.transpose()*point3D+keyFrame1->pose.translation;
points[3*(nPoints+i)]=point3D(0);
points[3*(nPoints+i)+1]=point3D(1);
points[3*(nPoints+i)+2]=point3D(2);
}
}
std::vector<bool> status(keyFrame1->mvLocalMapPoints.size(),false);
for(std::map<KeyFrame*,std::vector<int> >::iterator mit=keyFrame1->mConnectedKeyFrameMatches.begin(),
mend=keyFrame1->mConnectedKeyFrameMatches.end();
mit!=mend;
mit++){
KeyFrame* keyFrame2=mit->first;
if (keyFrame2->mvLocalFrames.empty()) {
continue;
}
Transform transform=keyFrame1->mConnectedKeyFramePoses[keyFrame2];
if (transform.scale<-10.0&&disable_loop) {
continue;
}
std::vector<int>& matches=mit->second;
assert(matches.size()==keyFrame1->mvLocalMapPoints.size());
for (int i=0;i<matches.size();i++) {
if (matches[i]>=0) {
assert(keyFrame1->mvLocalMapPoints[i].isEstimated);
Eigen::Vector3d projection(&points[3*(nPoints+i)]);
projection=keyFrame2->pose.rotation*(projection-keyFrame2->pose.translation);
projection/=projection(2);
projection-=keyFrame2->mvLocalMapPoints[matches[i]].vec;
if(!keyFrame2->mvLocalMapPoints[matches[i]].isEstimated&&projection.norm()>0.008){
continue;
}
ceres::CostFunction* cost_function = GlobalError2::Create(keyFrame2->mvLocalMapPoints[matches[i]].vec(0),
keyFrame2->mvLocalMapPoints[matches[i]].vec(1));
problem.AddResidualBlock(cost_function,loss_function,
&cameras[6*keyFrame2->mnId],
&points[3*(nPoints+i)]);
status[i]=true;
}
}
}
if(keyFrame1->nextKeyFramePtr!=NULL){
for(int i=0;i<keyFrame1->mvLocalMapPoints.size();i++){
Eigen::Vector3d projection(&points[3*(nPoints+i)]);
projection=keyFrame1->nextKeyFramePtr->pose.rotation*(projection-keyFrame1->nextKeyFramePtr->pose.translation);
projection/=projection(2);
projection-=(*keyFrame1->mvLocalMapPoints[i].vecs.back());
if (keyFrame1->mvLocalMapPoints[i].isEstimated
&&keyFrame1->mvLocalMapPoints[i].vecs.back()!=NULL&&projection.norm()<0.008) {
assert(keyFrame1->mvLocalMapPoints[i].vecs.size()==keyFrame1->mvLocalFrames.size());
Eigen::Vector3d projection=(*keyFrame1->mvLocalMapPoints[i].vecs.back());
projection/=projection(2);
ceres::CostFunction* cost_function = GlobalError2::Create(projection(0),projection(1));
problem.AddResidualBlock(cost_function,loss_function,
&cameras[6*keyFrame1->nextKeyFramePtr->mnId],
&points[3*(nPoints+i)]);
status[i]=true;
}
}
}
for (int i=0;i<keyFrame1->mvLocalMapPoints.size();i++) {
if (status[i]==true) {
ceres::CostFunction* cost_function = GlobalError2::Create(keyFrame1->mvLocalMapPoints[i].vec(0),
keyFrame1->mvLocalMapPoints[i].vec(1));
problem.AddResidualBlock(cost_function,loss_function,
&cameras[6*keyFrame1->mnId],
&points[3*(nPoints+i)]);
keyFrame1->mvLocalMapPoints[i].isEstimated=true;
}else{
keyFrame1->mvLocalMapPoints[i].isEstimated=false;
}
}
nPoints+=keyFrame1->mvLocalMapPoints.size();
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
options.minimizer_progress_to_stdout = true;
options.max_num_iterations=100;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
for(int i=0;i<keyFrames.size();i++){
Eigen::Matrix3d rotation;
ceres::AngleAxisToRotationMatrix(&cameras[6*i],
rotation.data());
Eigen::Vector3d translation;
translation(0)=cameras[6*i+3];
translation(1)=cameras[6*i+4];
translation(2)=cameras[6*i+5];
translation=-rotation.transpose()*translation;
keyFrames[i]->pose.rotation=rotation;
keyFrames[i]->pose.translation=translation;
}
nPoints=0;
for (int k=0;k<keyFrames.size();k++) {
std::vector<double> scales;
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) {
keyFrames[k]->mvLocalMapPoints[i].globalPosition(0)=points[3*(nPoints+i)];
keyFrames[k]->mvLocalMapPoints[i].globalPosition(1)=points[3*(nPoints+i)+1];
keyFrames[k]->mvLocalMapPoints[i].globalPosition(2)=points[3*(nPoints+i)+2];
Eigen::Vector3d globalDistance=keyFrames[k]->mvLocalMapPoints[i].globalPosition
-keyFrames[k]->pose.translation;
Eigen::Vector3d localDistance=keyFrames[k]->mvLocalMapPoints[i].getPosition();
scales.push_back(localDistance.norm()/globalDistance.norm());
}
}
std::sort(scales.begin(),scales.end());
keyFrames[k]->scale=scales[scales.size()/2];
nPoints+=keyFrames[k]->mvLocalMapPoints.size();
}
for(int i=1;i<keyFrames.size();i++){
keyFrames[i]->pose.rotation=keyFrames[i]->pose.rotation*keyFrames[0]->pose.rotation.transpose();
keyFrames[i]->pose.translation=keyFrames[0]->pose.rotation*(keyFrames[i]->pose.translation
-keyFrames[0]->pose.translation);
}
for (int k=0;k<keyFrames.size();k++) {
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) {
keyFrames[k]->mvLocalMapPoints[i].globalPosition=keyFrames[0]->pose.rotation*
(keyFrames[k]->mvLocalMapPoints[i].globalPosition-keyFrames[0]->pose.translation);
}
}
}
keyFrames[0]->pose.rotation=Eigen::Matrix3d::Identity();
keyFrames[0]->pose.translation=Eigen::Vector3d::Zero();
}
void GlobalReconstruction::addNewKeyFrame(KeyFrame* keyFrame){
keyFrames.push_back(keyFrame);
}
void GlobalReconstruction::savePly(){
ofstream results(path+std::string("/keyframes.txt"));
for (int k=0;k<keyFrames.size();k++) {
results<<keyFrames[k]->frameId<<' '<<keyFrames[k]->pose.translation.transpose();
for (int i1=0;i1<3;i1++) {
for (int i2=0;i2<3;i2++) {
results<<' '<<keyFrames[k]->pose.rotation(i1,i2);
}
}
results<<endl;
}
results.close();
ofstream trajectories(path+std::string("/frames.txt"));
for (int k=0;k<keyFrames.size();k++) {
for (int i=0;i<keyFrames[k]->mvLocalFrames.size();i++) {
Eigen::Matrix3d rotation;
rotation=keyFrames[k]->mvLocalFrames[i].pose.rotation*keyFrames[k]->pose.rotation;
Eigen::Vector3d translation=keyFrames[k]->mvLocalFrames[i].pose.translation;
translation=keyFrames[k]->pose.rotation.transpose()*translation;
translation/=keyFrames[k]->scale;
translation+=keyFrames[k]->pose.translation;
trajectories<<keyFrames[k]->mvLocalFrames[i].frameId<<' '<<translation.transpose();
for (int i1=0;i1<3;i1++) {
for (int i2=0;i2<3;i2++) {
trajectories<<' '<<rotation(i1,i2);
}
}
trajectories<<endl;
}
}
trajectories.close();
int npoints=0;
for (int k=0;k<keyFrames.size();k++) {
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
keyFrames[k]->mvLocalMapPoints[i].isEstimated=false;
}
for(map<KeyFrame*,vector<int> >::iterator mit=keyFrames[k]->mConnectedKeyFrameMatches.begin(),
mend=keyFrames[k]->mConnectedKeyFrameMatches.end();
mit!=mend;
mit++){
for (int i=0;i<mit->second.size();i++) {
if (mit->second[i]>=0) {
keyFrames[k]->mvLocalMapPoints[i].isEstimated=true;
npoints++;
}
}
}
}
ofstream pointcloud(path+std::string("/pointcloud.ply"));
pointcloud << "ply"
<< '\n' << "format ascii 1.0"
<< '\n' << "element vertex " <<npoints
<< '\n' << "property float x"
<< '\n' << "property float y"
<< '\n' << "property float z"
<< '\n' << "property uchar red"
<< '\n' << "property uchar green"
<< '\n' << "property uchar blue"
<< '\n' << "end_header" << std::endl;
for (int nFrame=0;nFrame<keyFrames.size();nFrame++) {
char name[200];
for(int k=0;k<=nFrame;k++){
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
if(keyFrames[k]->mvLocalMapPoints[i].isEstimated){
Eigen::Vector3d point3D=keyFrames[k]->mvLocalMapPoints[i].getPosition();
point3D/=keyFrames[k]->scale;
point3D=keyFrames[k]->pose.rotation.transpose()*point3D+keyFrames[k]->pose.translation;
uchar* color=keyFrames[k]->mvLocalMapPoints[i].color;
pointcloud <<point3D.transpose()<<' '<<(int)color[2]<<' '<<(int)color[1]<<' '<<(int)color[0]<<endl;
}
}
}
}
pointcloud.close();
}
void GlobalReconstruction::visualize(){
pangolin::WindowInterface& window=pangolin::CreateWindowAndBind("GSLAM: Map Viewer",720,720);
//window.Move(100,480);
// 3D Mouse handler requires depth testing to be enabled
glEnable(GL_DEPTH_TEST);
// Issue specific OpenGl we might need
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
float mViewpointX=0;
float mViewpointY=-0.7;
float mViewpointZ=-1.8;
float mViewpointF=500;
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ,0,0,0,0.0,-1.0, 0.0));
// Add named OpenGL viewport to window and provide 3D Handler
pangolin::View& d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
.SetHandler(new pangolin::Handler3D(s_cam));
pangolin::OpenGlMatrix Twc;
Twc.SetIdentity();
bool bFollow = true;
bool bLocalizationMode = false;
float mT = 1e3/30.0;
Drawer drawer;
drawer.mCameraSize=0.08;
drawer.mCameraLineWidth=3;
drawer.mFrameIndex=0;
drawer.mKeyFrameIndex=0;
drawer.keyFrames=keyFrames;
drawer.mGraphLineWidth=3.0;
// cv::namedWindow("track");
// cv::moveWindow("track",720,0);
//getchar();
drawer.preTwc.resize(1);
drawer.preTwc[0];
drawer.preTwc[0].m[12]=0.0;
drawer.preTwc[0].m[13]=0.0;
drawer.preTwc[0].m[14]=0.0;
cv::Mat preImage;
while(1){
if(drawer.mFrameIndex>=keyFrames.back()->mvLocalFrames.back().frameId){
break;
}
cv::Mat image;
if(drawer.mFrameIndex<keyFrames.back()->mvLocalFrames.back().frameId){
drawer.getCurrentOpenGLCameraMatrix(Twc);
// char name[200];
// sprintf(name,"%s/1080/frame%05d.pgm",path,drawer.mFrameIndex+frameStart+1);
// printf(name);
// image=cv::imread(name);
// printf("image %d %d\n",image.cols,image.rows);
// if ((drawer.mFrameIndex)==keyFrames[drawer.mKeyFrameIndex]->frameId) {
// cv::RNG rng(-1);
// for(int i=0;i<keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints.size();i++){
// if(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount<=0){
// continue;
// }
// //printf("%f %f %f\n",color.val[0],color.val[1],color[2]);
// uchar *color=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].color;
//
// int Size=min(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount,20);
// Size=max(Size,3);
// cv::circle(image,
// keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[0],Size,
// CV_RGB(245,211,40),2,CV_AA);
//
// }
// }else{
// for(int i=0;i<keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints.size();i++){
//
// if(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount<=0){
// continue;
// }
//
//
// if((drawer.mFrameIndex-keyFrames[drawer.mKeyFrameIndex]->frameId)
// >=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked.size()){
// continue;
// }
// int localIndex=drawer.mFrameIndex-keyFrames[drawer.mKeyFrameIndex]->frameId;
//
// int Size=min(keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].measurementCount,20);
// Size=max(Size,3);
// uchar *color=keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].color;
// cv::circle(image,keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[localIndex],Size,
// CV_RGB(245,211,40),2,CV_AA);
//
// for (int j=localIndex;j>max(localIndex-5,0);j--) {
// cv::line(image,
// keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[j],
// keyFrames[drawer.mKeyFrameIndex]->mvLocalMapPoints[i].tracked[j-1],
// CV_RGB(245,211,40),2,CV_AA);
// }
// }
// }
// cv::resize(image,image,cv::Size(720,405));
// image.copyTo(preImage);
s_cam.Follow(Twc);
}else{
preImage.copyTo(image);
s_cam.Follow(Twc);
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
glClearColor(1.0f,1.0f,1.0f,1.0f);
drawer.drawCurrentCamera(Twc);
drawer.drawKeyFrames();
drawer.drawPoints();
// char name[200];
// sprintf(name,"%s/viewer/viewr%05d",path,drawer.mFrameIndex);
// pangolin::SaveWindowOnRender(name);
// sprintf(name,"%s/viewer/track%05d.png",path,drawer.mFrameIndex);
// cv::imwrite(name,image);
// image.release();
pangolin::FinishFrame();
}
}
bool myfunction (const LocalMapPoint* p1,const LocalMapPoint* p2) { return (p1->invdepth<p2->invdepth); }
void GlobalReconstruction::topview(){
/*for (int k=0;k<keyFrames.size();k++) {
vector<LocalMapPoint*> localMapPoints;
for (int i=0;i<keyFrames[k]->mvLocalMapPoints.size();i++) {
if (keyFrames[k]->mvLocalMapPoints[i].isEstimated) {
localMapPoints.push_back(&keyFrames[k]->mvLocalMapPoints[i]);
}
}
sort(localMapPoints.begin(),localMapPoints.end(),myfunction);
for (int i=0;i<(0.8*localMapPoints.size());i++) {
localMapPoints[i]->isEstimated=false;
}
}*/
//getchar();
//pangolin::WindowInterface& window=pangolin::CreateWindowAndBind("GSLAM: Map Viewer",720,720);
//window.Move(100,480);
// 3D Mouse handler requires depth testing to be enabled
//glEnable(GL_DEPTH_TEST);
// Issue specific OpenGl we might need
//glEnable (GL_BLEND);
//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
float mViewpointX=0;
float mViewpointY=0;
float mViewpointZ=-1.8;
float mViewpointF=500;
for(int i=0;i<keyFrames.size();i++){
mViewpointX+=keyFrames[i]->pose.translation(0);
mViewpointY+=keyFrames[i]->pose.translation(1);
}
mViewpointX/=keyFrames.size();
mViewpointY/=keyFrames.size();
float maxdistance=0.0;
for (int i=1;i<keyFrames.size();i++) {
float distance=(float)keyFrames[i]->pose.translation.norm();
if (distance>maxdistance) {
maxdistance=distance;
}
}
printf("%f %f\n",mViewpointX,mViewpointY);
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
pangolin::ModelViewLookAt(0,
-maxdistance,
0,
0,0.0,0,
pangolin::AxisDirection::AxisZ));
// Add named OpenGL viewport to window and provide 3D Handler
pangolin::View& d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
.SetHandler(new pangolin::Handler3D(s_cam));
pangolin::OpenGlMatrix Twc;
Twc.SetIdentity();
bool bFollow = true;
bool bLocalizationMode = false;
float mT = 1e3/30.0;
Drawer drawer;
drawer.mCameraSize=0.5;
drawer.mCameraLineWidth=3;
drawer.mFrameIndex=0;
drawer.mKeyFrameIndex=0;
drawer.keyFrames=keyFrames;
drawer.mGraphLineWidth=5.0;
cv::namedWindow("track");
cv::moveWindow("track",720,0);
drawer.preTwc.resize(1);
drawer.preTwc[0];
drawer.preTwc[0].m[12]=0.0;
drawer.preTwc[0].m[13]=0.0;
drawer.preTwc[0].m[14]=0.0;
pangolin::OpenGlMatrix curretMatrix;
while(1){
if(drawer.mFrameIndex<keyFrames.back()->mvLocalFrames.back().frameId){
drawer.getCurrentOpenGLCameraMatrix(Twc);
}else{
break;
}
curretMatrix=s_cam.GetModelViewMatrix();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
glClearColor(1.0f,1.0f,1.0f,1.0f);
drawer.drawCurrentCamera(Twc);
drawer.drawKeyFrames();
drawer.drawPoints();
char name[200];
//sprintf(name,"%s/viewer/top%05d",path,drawer.mFrameIndex);
//pangolin::SaveWindowOnRender(name);
pangolin::FinishFrame();
}
drawer.mFrameIndex=0;
drawer.mKeyFrameIndex=0;
drawer.preTwc.resize(1);
drawer.preTwc[0];
drawer.preTwc[0].m[12]=0.0;
drawer.preTwc[0].m[13]=0.0;
drawer.preTwc[0].m[14]=0.0;
while(1){
if(drawer.mFrameIndex<keyFrames.back()->mvLocalFrames.back().frameId){
drawer.getCurrentOpenGLCameraMatrix(Twc);
}
s_cam.SetModelViewMatrix(curretMatrix);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
glClearColor(1.0f,1.0f,1.0f,1.0f);
drawer.drawCurrentCamera(Twc);
drawer.drawKeyFrames();
drawer.drawPoints();
// char name[200];
// sprintf(name,"%s/viewer/top%05d",path,drawer.mFrameIndex);
// pangolin::SaveWindowOnRender(name);
pangolin::FinishFrame();
}
}
}
================================================
FILE: GSLAM/GlobalReconstruction.h
================================================
//
// GlobalReconstruction.h
// GSLAM
//
// Created by ctang on 9/8/16.
// Copyright © 2016 ctang. All rights reserved.
//
#ifndef GlobalReconstruction_h
#define GlobalReconstruction_h
#include <vector>
#include "Eigen/Dense"
#include "ceres/rotation.h"
namespace GSLAM{
inline Eigen::Vector3d ComputeRelativeRotationError(const Eigen::Matrix3d& relative_rotation_matrix,
const Eigen::Matrix3d& rotation_matrix1,
const Eigen::Matrix3d& rotation_matrix2) {
// Compute the relative rotation error.
const Eigen::Matrix3d relative_rotation_matrix_error =rotation_matrix2.transpose()*relative_rotation_matrix*rotation_matrix1;
Eigen::Vector3d relative_rotation_error;
ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(relative_rotation_matrix_error.data()),
relative_rotation_error.data());
return relative_rotation_error;
}
inline void ApplyRotation(const Eigen::Vector3d& rotation_change,Eigen::Matrix3d& rotation_matrix) {
// Convert to rotation matrices.
Eigen::Matrix3d rotation_change_matrix;
ceres::AngleAxisToRotationMatrix(rotation_change.data(),ceres::ColumnMajorAdapter3x3(rotation_change_matrix.data()));
// Apply the rotation change.
rotation_matrix = rotation_matrix*rotation_change_matrix;
}
inline int max3(int a, int b, int c){
int m = a;
(m < b) && (m = b); //these are not conditional statements.
(m < c) && (m = c); //these are just boolean expressions.
return m;
}
typedef struct{
int variableIndex1;
int variableIndex2;
int keyFrameIndex1;
int keyFrameIndex2;
double value1;
double value12;
double weight;
bool isLoop;
}ScaleConstraint;
typedef struct{
int variableIndex1;
int variableIndex2;
int keyFrameIndex1;
int keyFrameIndex2;
Eigen::Matrix3d rotation1;//if 1 is fixed
Eigen::Matrix3d rotation12;
double weight;
bool isLoop;
}RotationConstraint;
typedef struct{
int variableIndex1;
int variableIndex2;
int keyFrameIndex1;
int keyFrameIndex2;
Eigen::Matrix3d rotation1;
Eigen::Vector3d translation1;//if 1 is fixed
Eigen::Vector3d translation12;
double weight;
bool isLoop;
}TranslationConstraint;
typedef struct{
int variableIndex1;
int variableIndex2;
int keyFrameIndex1;
int keyFrameIndex2;
Eigen::Matrix3d rotation12;
Eigen::Vector3d translation12;
double scale12;
double weight;
bool isLoop;
}SIM3Constraint;
class GlobalScaleEstimation{
public:
double *objective;
int *rowStart;
int *column;
double *rowUpper;
double *rowLower;
double *columnUpper;
double *columnLower;
double *elementByRow;
int maxIterations;
GlobalScaleEstimation(){
objective=NULL;
rowStart=NULL;
column=NULL;
rowUpper=NULL;
rowLower=NULL;
columnUpper=NULL;
columnLower=NULL;
elementByRow=NULL;
}
void solve(const std::vector<ScaleConstraint> &constraints,std::vector<double> &results);
void test();
};
class GlobalRotationEstimation{
public:
double *objective;
int *rowStart;
int *column;
double *rowUpper;
double *rowLower;
double *columnUpper;
double *columnLower;
double *elementByRow;
int maxOuterIterations;
int maxInnerIterations;
GlobalRotationEstimation(){
objective=NULL;
rowStart=NULL;
column=NULL;
rowUpper=NULL;
rowLower=NULL;
columnUpper=NULL;
columnLower=NULL;
elementByRow=NULL;
}
void solve(const std::vector<RotationConstraint> &constraints,std::vector<Eigen::Matrix3d>& results);
void test();
private:
};
class GlobalTranslationEstimation{
public:
double *objective;
int *rowStart;
int *column;
double *rowUpper;
double *rowLower;
double *columnUpper;
double *columnLower;
double *elementByRow;
int maxIterations;
GlobalTranslationEstimation(){
objective=NULL;
rowStart=NULL;
column=NULL;
rowUpper=NULL;
rowLower=NULL;
columnUpper=NULL;
columnLower=NULL;
elementByRow=NULL;
}
void solve(const std::vector<TranslationConstraint> &constraints,std::vector<Eigen::Vector3d>& results);
void solveComplex(const std::vector<TranslationConstraint> &constraints,std::vector<Eigen::Vector3d>& results);
void test();
};
class KeyFrame;
class GlobalReconstruction{
std::vector<KeyFrame*> keyFrames;
GlobalRotationEstimation globalRotationEstimation;
GlobalScaleEstimation globalScaleEstimation;
GlobalTranslationEstimation globalTranslationEstimation;
void getScaleConstraint(KeyFrame* keyFrame1,std::vector<ScaleConstraint>& scaleConstraints);
void getRotationConstraint(KeyFrame* keyFrame1,std::vector<RotationConstraint>& rotationConstraints);
void getTranslationConstraint(KeyFrame* keyFrame1,std::vector<TranslationConstraint>& translationConstraints);
void getSIM3Constraint(KeyFrame* keyFrame1,std::vector<SIM3Constraint>& constraints);
public:
void saveTUM(const char* savename,const std::vector<double>& timestamps);
void globalRefine();
char *path;
int frameStart;
GlobalReconstruction(){
keyFrames.clear();
}
int scaleThreshold;
void addNewKeyFrame(KeyFrame* keyFrame);
void estimateScale();
void estimateRotation(std::vector<int>& rotationIndex);
void estimateRotationRobust(const std::vector<int>& rotationIndex);
void estimateTranslation(const std::vector<int>& translationIndex);
void estimateSIM3();
void savePly();
void visualize();
void topview();
};
}
#endif /* GlobalReconstruction_h */
================================================
FILE: GSLAM/GlobalRotationEstimation.cpp
================================================
//
// GlobalRotationEstimation.cpp
// GSLAM
//
// Created by ctang on 10/2/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include "GlobalReconstruction.h"
#include "L1Solver.h"
#include "opencv2/core/core.hpp"
#include <sys/time.h>
namespace GSLAM{
void GlobalRotationEstimation::solve(const std::vector<RotationConstraint>& constraints,
std::vector<Eigen::Matrix3d>& results){
static const double kConvergenceThreshold = 1e-2;
int variableCount=results.size();
int constraintCount=constraints.size();
int rowCount=2*constraintCount;
int columnCount=variableCount+constraintCount;
int singleVariableConstraintCount=0;
for(int i=0;i<constraintCount;i++){
singleVariableConstraintCount+=(constraints[i].variableIndex1==-1);
assert(constraints[i].variableIndex2!=-1);
}
int elementCount=3*rowCount-2*singleVariableConstraintCount;
if (objective==NULL) {
objective=(double*)malloc(columnCount*sizeof(double));
rowStart=(int*)malloc((rowCount+1)*sizeof(int));
column=(int*)malloc(elementCount*sizeof(int));
rowUpper=(double*)malloc(rowCount*sizeof(double));
rowLower=(double*)malloc(rowCount*sizeof(double));
columnUpper=(double*)malloc(columnCount*sizeof(double));
columnLower=(double*)malloc(columnCount*sizeof(double));
elementByRow=(double*)malloc(elementCount*sizeof(double));
}else{
objective=(double*)realloc(objective,columnCount*sizeof(double));
rowStart=(int*)realloc(rowStart,(rowCount+1)*sizeof(int));
column=(int*)realloc(column,elementCount*sizeof(int));
rowUpper=(double*)realloc(rowUpper,rowCount*sizeof(double));
rowLower=(double*)realloc(rowLower,rowCount*sizeof(double));
columnUpper=(double*)realloc(columnUpper,columnCount*sizeof(double));
columnLower=(double*)realloc(columnLower,columnCount*sizeof(double));
elementByRow=(double*)realloc(elementByRow,elementCount*sizeof(double));
}
for (int i = 0; i < variableCount; ++i){
columnLower[i]=-COIN_DBL_MAX;
columnUpper[i]=COIN_DBL_MAX;
}
for(int i=variableCount;i<columnCount;i++){
columnLower[i]=0.0;
columnUpper[i]=COIN_DBL_MAX;
}
int elementIndex=0;
int *rowStartPtr=&rowStart[0];
int constraintOffset=variableCount;
for (int c = 0; c < constraintCount; c++){
rowStartPtr[2*c]=elementIndex;
if(constraints[c].variableIndex1!=-1){
column[elementIndex]=constraints[c].variableIndex1;
elementByRow[elementIndex++]=-1;
}
if(constraints[c].variableIndex2!=-1){
column[elementIndex]=constraints[c].variableIndex2;
elementByRow[elementIndex++]=1;
}
column[elementIndex]=constraintOffset+c;
elementByRow[elementIndex++]=-1;
rowStartPtr[2*c+1]=elementIndex;
if(constraints[c].variableIndex1!=-1){
column[elementIndex]=constraints[c].variableIndex1;
elementByRow[elementIndex++]=-1;
}
if(constraints[c].variableIndex2!=-1){
column[elementIndex]=constraints[c].variableIndex2;
elementByRow[elementIndex++]=1;
}
column[elementIndex]=constraintOffset+c;
elementByRow[elementIndex++]=1;
}
rowStart[rowCount]=elementIndex;
assert(elementIndex==elementCount);
memset(objective,0,variableCount*sizeof(double));
for(int i=variableCount;i<columnCount;i++){
objective[i]=constraints[i-variableCount].weight;
}
ClpSimplex model;
CoinPackedMatrix byRow(false,columnCount,rowCount,elementCount,
elementByRow,column,rowStart,NULL);
std::vector<Eigen::Vector3d> rotationErrors(constraintCount);
std::vector<Eigen::Vector3d> rotationUpdates(variableCount);
double rotationError=0.0;
for (int iter=0;iter<maxOuterIterations;iter++) {
//evaluate error
for (int i=0;i<constraintCount;i++) {
assert(constraints[i].variableIndex2!=-1);
if (constraints[i].variableIndex1!=-1) {
/*std::cout<<constraints[i].variableIndex1<<' '<<constraints[i].variableIndex2<<std::endl;
std::cout<<constraints[i].rotation12<<std::endl;
std::cout<<results[constraints[i].variableIndex1]<<std::endl;
std::cout<<results[constraints[i].variableIndex2]<<std::endl;*/
rotationErrors[i]=ComputeRelativeRotationError(constraints[i].rotation12,
results[constraints[i].variableIndex1],
results[constraints[i].variableIndex2]);
}else{
rotationErrors[i]=ComputeRelativeRotationError(constraints[i].rotation12,
constraints[i].rotation1,
results[constraints[i].variableIndex2]);
}
rotationError+=rotationErrors[i].norm();
}
rotationError/=constraintCount;
if (rotationError<kConvergenceThreshold) {
break;
}
for (int r=0;r<3;r++) {
for (int i=0;i<constraintCount;i++) {
rowUpper[2*i] =rotationErrors[i](r);
rowLower[2*i] =-COIN_DBL_MAX;
rowUpper[2*i+1] =COIN_DBL_MAX;
rowLower[2*i+1] =rotationErrors[i](r);
}
model.setLogLevel(0);
model.loadProblem(byRow,columnLower,columnUpper,objective,rowLower,rowUpper);
model.setMaximumIterations(maxInnerIterations);
model.dual();
const double *solutionPtr=model.primalColumnSolution();
for (int i=0;i<variableCount;i++) {
rotationUpdates[i](r)=solutionPtr[i];
}
}
for (int i=0;i<variableCount;i++) {
ApplyRotation(rotationUpdates[i],results[i]);
}
maxInnerIterations*=2;
}
}
void GlobalRotationEstimation::test(){
int num_rotations=1000;
std::vector<Eigen::Matrix3d> globalRotations(num_rotations);
std::vector<RotationConstraint> constraints;
cv::RNG rng(-1);
for (int i=0;i<globalRotations.size();i++) {
double angles[3]={rng.uniform(-10.0,10.0),rng.uniform(-10.0,10.0),rng.uniform(-10.0,10.0)};
for (int r=0;r<3;r++) {
angles[r]*=(CV_PI/180.0);
}
ceres::AngleAxisToRotationMatrix(angles,ceres::ColumnMajorAdapter3x3(globalRotations[i].data()));
}
for (int i=0;i<num_rotations;i++) {
RotationConstraint constraint;
constraint.variableIndex1=i;
for (int j=i+1;j<std::min(i+5,num_rotations);j++) {
constraint.variableIndex2=j;
constraint.rotation12=globalRotations[j]*globalRotations[i].transpose();
constraint.weight=1.0;
constraints.push_back(constraint);
}
if (i!=0) {
continue;
}
constraint.variableIndex1=-1;
constraint.variableIndex2=i;
constraint.rotation1=Eigen::Matrix3d::Identity();
constraint.rotation12=globalRotations[i];
constraint.weight=1.0;
constraints.push_back(constraint);
if (i<num_rotations&&i>num_rotations-100) {
constraint.variableIndex1=0;
constraint.variableIndex2=i;
constraint.rotation12=globalRotations[i]*globalRotations[0].transpose();
constraint.weight=1.0;
constraints.push_back(constraint);
}
}
for (int i=0;i<constraints.size();i++) {
double angles[3]={rng.uniform(-0.5,0.5),rng.uniform(-0.5,0.5),rng.uniform(-0.5,0.5)};
for (int r=0;r<3;r++) {
angles[r]*=(CV_PI/180.0);
}
Eigen::Matrix3d noisyRotation;
ceres::AngleAxisToRotationMatrix(angles,ceres::ColumnMajorAdapter3x3(noisyRotation.data()));
constraints[i].rotation12=noisyRotation*constraints[i].rotation12;
}
std::vector<Eigen::Matrix3d> results(6,Eigen::Matrix3d::Identity());
int i=0;
std::vector<RotationConstraint> incrementalConstraints;
for (int f=5;f<num_rotations;f++) {
for (;i<constraints.size();i++) {
if (constraints[i].variableIndex2<=f) {
incrementalConstraints.push_back(constraints[i]);
}else{
break;
}
}
if (f==5) {
this->solve(incrementalConstraints,results);
}else{
std::vector<double> wx,wy,wz;
for (int j=incrementalConstraints.size()-1;j>0;j--) {
if (incrementalConstraints[j].variableIndex2==f) {
const Eigen::Matrix3d rotationf=incrementalConstraints[j].rotation12*results[incrementalConstraints[j].variableIndex1];
double angles[3];
ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(rotationf.data()),angles);
wx.push_back(angles[0]);
wy.push_back(angles[1]);
wz.push_back(angles[2]);
}
}
std::sort(wx.begin(),wx.end());
std::sort(wy.begin(),wy.end());
std::sort(wz.begin(),wz.end());
const double angles[3]={wx[wx.size()/2],wy[wy.size()/2],wz[wz.size()/2]};
Eigen::Matrix3d rotationf;
ceres::AngleAxisToRotationMatrix(angles,ceres::ColumnMajorAdapter3x3(rotationf.data()));
results.push_back(rotationf);
this->solve(incrementalConstraints,results);
}
}
std::vector<Eigen::Matrix3d> results2(num_rotations,Eigen::Matrix3d::Identity());
this->solve(constraints,results2);
double error0=0.0,error1=0.0,error2=0;
for (int i=1;i<num_rotations;i++) {
std::cout<<"****"<<i<<"****"<<std::endl;
std::cout<<results[i]*results[0].transpose()<<std::endl;
std::cout<<"**********"<<std::endl;
std::cout<<globalRotations[i]*globalRotations[0].transpose()<<std::endl;
std::cout<<"**********"<<std::endl;
std::cout<<results2[i]*results2[0].transpose()<<std::endl;
std::cout<<"**********"<<std::endl;
const Eigen::Matrix3d rotationError0=globalRotations[0]*globalRotations[i].transpose()*results[i]*results[0].transpose();
const Eigen::Matrix3d rotationError1=globalRotations[0]*globalRotations[i].transpose()*results2[i]*results2[0].transpose();
const Eigen::Matrix3d rotationError2=globalRotations[i].transpose()*results2[i];
double q[4];
ceres::RotationMatrixToQuaternion(ceres::ColumnMajorAdapter3x3(rotationError0.data()),q);
error0+=std::abs(q[3]);
ceres::RotationMatrixToQuaternion(ceres::ColumnMajorAdapter3x3(rotationError1.data()),q);
error1+=std::abs(q[3]);
ceres::RotationMatrixToQuaternion(ceres::ColumnMajorAdapter3x3(rotationError2.data()),q);
error2+=std::abs(q[3]);
}
printf("%f %f %f\n",error0/num_rotations,error1/num_rotations,error2/num_rotations);
}
}
================================================
FILE: GSLAM/GlobalScaleEstimation.cpp
================================================
//
// GlobalScaleEstimation.cpp
// GSLAM
//
// Created by ctang on 9/29/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include "GlobalReconstruction.h"
#include "L1Solver.h"
#include "opencv2/core/core.hpp"
namespace GSLAM{
void GlobalScaleEstimation::solve(const std::vector<ScaleConstraint>& constraints,std::vector<double>& results){
//timeval time;
//gettimeofday(&time, NULL);
//long millis = (time.tv_sec * 1000) + (time.tv_usec / 1000);
int variableCount=results.size();
int constraintCount=constraints.size();
int rowCount=2*constraintCount;
int columnCount=variableCount+constraintCount;
int singleVariableConstraintCount=0;
for(int i=0;i<constraintCount;i++){
singleVariableConstraintCount+=(constraints[i].variableIndex1==-1);
assert(constraints[i].variableIndex2!=-1);
}
int elementCount=3*rowCount-2*singleVariableConstraintCount;
if (objective==NULL) {
objective=(double*)malloc(columnCount*sizeof(double));
rowStart=(int*)malloc((rowCount+1)*sizeof(int));
column=(int*)malloc(elementCount*sizeof(int));
rowUpper=(double*)malloc(rowCount*sizeof(double));
rowLower=(double*)malloc(rowCount*sizeof(double));
columnUpper=(double*)malloc(columnCount*sizeof(double));
columnLower=(double*)malloc(columnCount*sizeof(double));
elementByRow=(double*)malloc(elementCount*sizeof(double));
}else{
objective=(double*)realloc(objective,columnCount*sizeof(double));
rowStart=(int*)realloc(rowStart,(rowCount+1)*sizeof(int));
column=(int*)realloc(column,elementCount*sizeof(int));
rowUpper=(double*)realloc(rowUpper,rowCount*sizeof(double));
rowLower=(double*)realloc(rowLower,rowCount*sizeof(double));
columnUpper=(double*)realloc(columnUpper,columnCount*sizeof(double));
columnLower=(double*)realloc(columnLower,columnCount*sizeof(double));
elementByRow=(double*)realloc(elementByRow,elementCount*sizeof(double));
}
for (int i = 0; i <constraintCount; ++i){
rowUpper[2*i]=constraints[i].value12;
rowLower[2*i]=-COIN_DBL_MAX;
rowUpper[2*i+1]=COIN_DBL_MAX;
rowLower[2*i+1]=constraints[i].value12;
if (constraints[i].variableIndex1==-1) {
rowUpper[2*i]+=constraints[i].value1-results[constraints[i].variableIndex2];
rowLower[2*i+1]+=constraints[i].value1-results[constraints[i].variableIndex2];
}else{
rowUpper[2*i]+=results[constraints[i].variableIndex1]-results[constraints[i].variableIndex2];
rowLower[2*i+1]+=results[constraints[i].variableIndex1]-results[constraints[i].variableIndex2];
}
}
for (int i = 0; i < variableCount; ++i){
columnLower[i]=-COIN_DBL_MAX;
columnUpper[i]=COIN_DBL_MAX;
}
for(int i=variableCount;i<columnCount;i++){
columnLower[i]=0.0;
columnUpper[i]=COIN_DBL_MAX;
}
int elementIndex=0;
int *rowStartPtr=&rowStart[0];
int constraintOffset=variableCount;
for (int c = 0; c < constraintCount; c++){
rowStartPtr[2*c]=elementIndex;
if(constraints[c].variableIndex1!=-1){
column[elementIndex]=constraints[c].variableIndex1;
elementByRow[elementIndex++]=-1;
}
assert(constraints[c].variableIndex2!=-1);
column[elementIndex]=constraints[c].variableIndex2;
elementByRow[elementIndex++]=1;
column[elementIndex]=constraintOffset+c;
elementByRow[elementIndex++]=-1;
rowStartPtr[2*c+1]=elementIndex;
if(constraints[c].variableIndex1!=-1){
column[elementIndex]=constraints[c].variableIndex1;
elementByRow[elementIndex++]=-1;
}
assert(constraints[c].variableIndex2!=-1);
column[elementIndex]=constraints[c].variableIndex2;
elementByRow[elementIndex++]=1;
column[elementIndex]=constraintOffset+c;
elementByRow[elementIndex++]=1;
}
rowStart[rowCount]=elementIndex;
assert(elementIndex==elementCount);
memset(objective,0,variableCount*sizeof(double));
for(int i=variableCount;i<columnCount;i++){
objective[i]=constraints[i-variableCount].weight;
}
ClpSimplex model;
CoinPackedMatrix byRow(false,columnCount,rowCount,elementCount,
elementByRow,column,rowStart,NULL);
model.setMaximumIterations(this->maxIterations);
model.loadProblem(byRow,columnLower,columnUpper,objective,rowLower,rowUpper);
model.dual();
const double *solutionPtr=model.primalColumnSolution();
for (int i=0;i<variableCount;i++) {
//printf("%d %f\n",i,solutionPtr[i]);
results[i]+=solutionPtr[i];
}
//gettimeofday(&time, NULL);
//long millis2 = (time.tv_sec * 1000) + (time.tv_usec / 1000);
//getchar();
//printf("%d ms\n",millis2-millis);
}
void GlobalScaleEstimation::test(){
int num_scales=500;
std::vector<double> globalScales(num_scales);
cv::RNG rng(-1);
for (int i=0;i<num_scales;i++) {
globalScales[i]=rng.uniform(1.0,10.0);
}
std::vector<ScaleConstraint> constraints;
for (int i=0;i<num_scales;i++) {
ScaleConstraint constraint;
constraint.variableIndex1=i;
for (int j=i+1;j<std::min(i+5,num_scales);j++) {
constraint.variableIndex2=j;
constraint.value12=std::log(globalScales[j]/globalScales[i]);
constraint.weight=1.0;
constraints.push_back(constraint);
}
if (i<num_scales&&i>num_scales-100) {
constraint.variableIndex1=-1;
constraint.variableIndex2=i;
constraint.value1=0.0;
constraint.value12=std::log(globalScales[i]);
constraint.weight=1.0;
constraints.push_back(constraint);
}
if (i!=0) {
continue;
}
constraint.variableIndex1=-1;
constraint.variableIndex2=i;
constraint.value1=0.0;
constraint.value12=std::log(globalScales[i]);
constraint.weight=1.0;
constraints.push_back(constraint);
}
for (int i=0;i<constraints.size();i++) {
constraints[i].value12+=std::log(rng.uniform(0.95,1.05));
}
int i=0;
std::vector<ScaleConstraint> incrementalConstraints;
std::vector<double> results2(6,0);
for (int f=5;f<num_scales;f++) {
for (;i<constraints.size();i++) {
if (constraints[i].variableIndex2<=f) {
incrementalConstraints.push_back(constraints[i]);
}else{
break;
}
}
if (f==5) {
this->solve(incrementalConstraints,results2);
}else{
std::vector<double> v;
for (int j=incrementalConstraints.size()-1;j>0;j--) {
if (incrementalConstraints[j].variableIndex2==f) {
v.push_back(incrementalConstraints[j].value12+results2[incrementalConstraints[j].variableIndex1]);
}
}
std::sort(v.begin(),v.end());
results2.push_back(v[v.size()/2]);
this->maxIterations=1000;
this->solve(incrementalConstraints,results2);
}
}
this->maxIterations=10000;
this->solve(incrementalConstraints,results2);
this->maxIterations=10000;
std::vector<double> results(num_scales,0);
this->solve(constraints,results);
results2.resize(results.size());
for (int i=1;i<results.size();i++) {
std::cout<<std::exp(results2[i]-results2[0])<<' '<<std::exp(results[i]-results[0])<<' '<<globalScales[i]/globalScales[0]<<std::endl;
}
double error0=0,error1=0,error2=0;
for (int i=1;i<results.size();i++) {
//error0+=std::abs(std::exp(results2[i]-results2[0])-globalScales[i]/globalScales[0]);
//error1+=std::abs(std::exp(results[i]-results[0])-globalScales[i]/globalScales[0]);
//error2+=std::abs(std::exp(results[i])-globalScales[i]);
error0+=std::abs(results2[i]-results2[0]-std::log(globalScales[i]/globalScales[0]));
error1+=std::abs(results[i]-results[0]-std::log(globalScales[i]/globalScales[0]));
error2+=std::abs(results[i]-std::log(globalScales[i]));
}
std::cout<<error0/500<<' '<<error1/500<<' '<<error2/500<<std::endl;
}
}
================================================
FILE: GSLAM/GlobalTranslationEstimation.cpp
================================================
//
// GlobalTranslationEstimation.cpp
// GSLAM
//
// Created by ctang on 10/3/16.
// Copyright © 2016 ctang. All rights reserved.
//
#include "GlobalReconstruction.h"
#include "L1Solver.h"
#include "opencv2/core/core.hpp"
namespace GSLAM{
void GlobalTranslationEstimation::solve(const std::vector<TranslationConstraint> &constraints,std::vector<Eigen::Vector3d>& results){
int variableCount=results.size();
int constraintCount=constraints.size();
int rowCount=2*constraintCount;
int columnCount=variableCount+constraintCount;
int singleVariableConstraintCount=0;
for(int i=0;i<constraintCount;i++){
singleVariableConstraintCount+=(constraints[i].variableIndex1==-1);
assert(constraints[i].variableIndex2!=-1);
}
int elementCount=3*rowCount-2*singleVariableConstraintCount;
if (objective==NULL) {
objective=(double*)malloc(columnCount*sizeof(double));
rowStart=(int*)malloc((rowCount+1)*sizeof(int));
column=(int*)malloc(elementCount*sizeof(int));
rowUpper=(double*)malloc(rowCount*sizeof(double));
rowLower=(double*)malloc(rowCount*sizeof(double));
columnUpper=(double*)malloc(columnCount*sizeof(double));
columnLower=(double*)malloc(columnCount*sizeof(double));
elementByRow=(double*)malloc(elementCount*sizeof(double));
}else{
objective=(double*)realloc(objective,columnCount*sizeof(double));
rowStart=(int*)realloc(rowStart,(rowCount+1)*sizeof(int));
column=(int*)realloc(column,elementCount*sizeof(int));
rowUpper=(double*)realloc(rowUpper,rowCount*sizeof(double));
rowLower=(double*)realloc(rowLower,rowCount*sizeof(double));
columnUpper=(double*)realloc(columnUpper,columnCount*sizeof(double));
columnLower=(double*)realloc(columnLower,columnCount*sizeof(double));
elementByRow=(double*)realloc(elementByRow,elementCount*sizeof(double));
}
for (int i = 0; i < variableCount; ++i){
columnLower[i]=-COIN_DBL_MAX;
columnUpper[i]=COIN_DBL_MAX;
}
for(int i=variableCount;i<columnCount;i++){
columnLower[i]=0.0;
columnUpper[i]=COIN_DBL_MAX;
}
int elementIndex=0;
int *rowStartPtr=&rowStart[0];
int constraintOffset=variableCount;
for (int c = 0; c < constraintCount; c++){
rowStartPtr[2*c]=elementIndex;
if(constraints[c].variableIndex1!=-1){
column[elementIndex]=constraints[c].variableIndex1;
elementByRow[elementIndex++]=-1;
}
assert(constraints[c].variableIndex2!=-1);
column[elementIndex]=constraints[c].variableIndex2;
elementByRow[elementIndex++]=1;
column[elementIndex]=constraintOffset+c;
elementByRow[elementIndex++]=-1;
rowStartPtr[2*c+1]=elementIndex;
if(constraints[c].variableIndex1!=-1){
column[elementIndex]=constraints[c].variableIndex1;
elementByRow[elementIndex++]=-1;
}
assert(constraints[c].variableIndex2!=-1);
column[elementIndex]=constraints[c].variableIndex2;
elementByRow[elementIndex++]=1;
column[elementIndex]=constraintOffset+c;
elementByRow[elementIndex++]=1;
}
rowStart[rowCount]=elementIndex;
assert(elementIndex==elementCount);
memset(objective,0,variableCount*sizeof(double));
for(int i=variableCount;i<columnCount;i++){
objective[i]=constraints[i-variableCount].weight;
}
ClpSimplex model;
CoinPackedMatrix byRow(false,columnCount,rowCount,elementCount,
elementByRow,column,rowStart,NULL);
//model.setLogLevel(0);
//model.setMaximumIterations(this->maxIterations);
for (int c=0;c<3;c++) {
for (int i = 0; i <constraintCount; ++i){
rowUpper[2*i]=constraints[i].translation12(c);
rowLower[2*i]=-COIN_DBL_MAX;
rowUpper[2*i+1]=COIN_DBL_MAX;
rowLower[2*i+1]=constraints[i].translation12(c);
if (constraints[i].variableIndex1==-1) {
rowUpper[2*i]+=constraints[i].translation1(c)-results[constraints[i].variableIndex2](c);
rowLower[2*i+1]+=constraints[i].translation1(c)-results[constraints[i].variableIndex2](c);
}else{
rowUpper[2*i]+=results[constraints[i].variableIndex1](c)-results[constraints[i].variableIndex2](c);
rowLower[2*i+1]+=results[constraints[i].variableIndex1](c)-results[constraints[i].variableIndex2](c);
}
}
model.loadProblem(byRow,columnLower,columnUpper,objective,rowLower,rowUpper);
model.primal();
const double *solutionPtr=model.primalColumnSolution();
for (int i=0;i<variableCount;i++) {
results[i](c)=solutionPtr[i];
}
}
}
void GlobalTranslationEstimation::solveComplex(const std::vector<TranslationConstraint> &constraints,std::vector<Eigen::Vector3d>& results){
}
void GlobalTranslationEstimation::test(){
int num_translation=500;
std::vector<Eigen::Vector3d> globalTranslations(num_translation);
std::vector<Eigen::Matrix3d> globalRotations(num_translation);
cv::RNG rng(-1);
for (int i=0;i<num_translation;i++) {
globalTranslations[i](0)=rng.uniform(-100.0,100.0);
globalTranslations[i](1)=rng.uniform(-100.0,100.0);
globalTranslations[i](2)=rng.uniform(-100.0,100.0);
double angles[3]={rng.uniform(-10.0,10.0),rng.uniform(-10.0,10.0),rng.uniform(-10.0,10.0)};
for (int r=0;r<3;r++) {
angles[r]*=(CV_PI/180.0);
}
ceres::AngleAxisToRotationMatrix(angles,ceres::ColumnMajorAdapter3x3(globalRotations[i].data()));
}
std::vector<TranslationConstraint> constraints;
for (int i=0;i<num_translation;i++) {
TranslationConstraint constraint;
constraint.variableIndex1=i;
constraint.rotation1=globalRotations[i];
for (int j=i+1;j<std::min(i+5,num_translation);j++) {
constraint.variableIndex2=j;
constraint.translation12=globalTranslations[j]-globalTranslations[i];
constraint.weight=1.0;
constraints.push_back(constraint);
}
if (i!=0) {
continue;
}
constraint.variableIndex1=-1;
constraint.variableIndex2=i;
constraint.translation1=Eigen::Vector3d::Zero();
constraint.translation12=globalTranslations[i];
constraint.weight=1.0;
constraints.push_back(constraint);
}
for (int i=0;i<num_translation;i++) {
constraints[i].translation12(0)+=rng.uniform(-2.0,2.0);
constraints[i].translation12(1)+=rng.uniform(-2.0,2.0);
constraints[i].translation12(2)+=rng.uniform(-2.0,2.0);
}
std::vector<Eigen::Vector3d> results(num_translation,Eigen::Vector3d::Zero());
this->solve(constraints,results);
double error0=0;
double error1=0;
for (int i=0;i<num_translation;i++) {
//std::cout<<i<<'\n'<<globalTranslations[i]-globalTranslations[0]<<'\n'<<results[i]-results[0]<<std::endl;
error0+=(globalTranslations[i]-results[i]).norm();
error1+=(globalTranslations[i]-globalTranslations[0]-results[i]+results[0]).norm();
}
std::cout<<error0<<' '<<error1<<std::endl;
}
}
================================================
FILE: GSLAM/Homography.hpp
================================================
//
// Homography.hpp
// GSLAM
//
// Created by Chaos on 2017-02-09.
// Copyright © 2017 ctang. All rights reserved.
//
#ifndef Homography_h
#define Homography_h
#pragma once
#include "opencv2/core/core.hpp"
#include "opencv2/calib3d/calib3d.hpp"
namespace GSLAM{
static void icvGetRTMatrix(const CvPoint2D32f* a,
const CvPoint2D32f* b,
int count, CvMat* M,
int full_affine){
if (full_affine)
{
double sa[36], sb[6];
CvMat A = cvMat(6, 6, CV_64F, sa), B = cvMat(6, 1, CV_64F, sb);
CvMat MM = cvMat(6, 1, CV_64F, M->data.db);
int i;
memset(sa, 0, sizeof(sa));
memset(sb, 0, sizeof(sb));
for (i = 0; i < count; i++)
{
sa[0] += a[i].x*a[i].x;
sa[1] += a[i].y*a[i].x;
sa[2] += a[i].x;
sa[6] += a[i].x*a[i].y;
sa[7] += a[i].y*a[i].y;
sa[8] += a[i].y;
sa[12] += a[i].x;
sa[13] += a[i].y;
sa[14] += 1;
sb[0] += a[i].x*b[i].x;
sb[1] += a[i].y*b[i].x;
sb[2] += b[i].x;
sb[3] += a[i].x*b[i].y;
sb[4] += a[i].y*b[i].y;
sb[5] += b[i].y;
}
sa[21] = sa[0];
sa[22] = sa[1];
sa[23] = sa[2];
sa[27] = sa[6];
sa[28] = sa[7];
sa[29] = sa[8];
sa[33] = sa[12];
sa[34] = sa[13];
sa[35] = sa[14];
cvSolve(&A, &B, &MM, CV
gitextract_116388qb/ ├── GSLAM/ │ ├── Drawer copy.h │ ├── Drawer.cpp │ ├── Drawer.h │ ├── Estimation.cpp │ ├── Estimation.h │ ├── Frame.cpp │ ├── Frame.h │ ├── Geometry.cpp │ ├── Geometry.h │ ├── GlobalReconstruction.cpp │ ├── GlobalReconstruction.h │ ├── GlobalRotationEstimation.cpp │ ├── GlobalScaleEstimation.cpp │ ├── GlobalTranslationEstimation.cpp │ ├── Homography.hpp │ ├── IMU.hpp │ ├── ImageGrids.hpp │ ├── KLT.h │ ├── KLTUtil.cpp │ ├── KLTUtil.h │ ├── KeyFrame.cpp │ ├── KeyFrame.h │ ├── KeyFrameConnection.cpp │ ├── KeyFrameConnection.h │ ├── KeyFrameDatabase.cpp │ ├── KeyFrameDatabase.h │ ├── L1Solver.h │ ├── LK.hpp │ ├── MapPoint.cpp │ ├── MapPoint.h │ ├── ORBVocabulary.h │ ├── ORBextractor.cc │ ├── ORBextractor.h │ ├── ORBmatcher.cc │ ├── ORBmatcher.h │ ├── RelativeMotion.hpp │ ├── Settings.h │ ├── System.cpp │ ├── System.h │ ├── TrackUtil.cpp │ ├── TrackUtil.h │ ├── Transform.cpp │ ├── Transform.h │ ├── convolve.cpp │ ├── convolve.h │ ├── error.cpp │ ├── error.h │ ├── experiment_helpers.hpp │ ├── las2.cpp │ ├── main.cpp │ ├── pyramid.cpp │ ├── pyramid.h │ ├── random_generators.hpp │ ├── selectGoodFeatures.hpp │ ├── selectGoodFeatures2.hpp │ ├── svdlib.cpp │ ├── svdlib.hpp │ ├── svdutil.cpp │ ├── svdutil.hpp │ └── time_measurement.hpp ├── GSLAM.xcodeproj/ │ ├── project.pbxproj │ ├── project.xcworkspace/ │ │ ├── contents.xcworkspacedata │ │ └── xcuserdata/ │ │ ├── chaos.xcuserdatad/ │ │ │ └── UserInterfaceState.xcuserstate │ │ └── ctang.xcuserdatad/ │ │ └── UserInterfaceState.xcuserstate │ └── xcuserdata/ │ ├── chaos.xcuserdatad/ │ │ ├── xcdebugger/ │ │ │ └── Breakpoints_v2.xcbkptlist │ │ └── xcschemes/ │ │ ├── GSLAM.xcscheme │ │ └── xcschememanagement.plist │ └── ctang.xcuserdatad/ │ ├── xcdebugger/ │ │ └── Breakpoints_v2.xcbkptlist │ └── xcschemes/ │ ├── GSLAM.xcscheme │ └── xcschememanagement.plist └── README.md
SYMBOL INDEX (296 symbols across 51 files)
FILE: GSLAM/Drawer copy.h
function namespace (line 14) | namespace GSLAM
function namespace (line 36) | namespace GSLAM{
FILE: GSLAM/Drawer.h
function namespace (line 17) | namespace GSLAM
function namespace (line 42) | namespace GSLAM{
FILE: GSLAM/Estimation.cpp
function getPVector (line 19) | bool getPVector(const Eigen::Vector3d &T12,
function getScaleRatio (line 62) | void getScaleRatio(const Eigen::Vector3d &T12,
function estimateRelativeTranslationIRLS (line 94) | Eigen::Vector3d estimateRelativeTranslationIRLS(const std::vector<Eigen:...
function estimateRelativeTranslation (line 194) | Eigen::Vector3d estimateRelativeTranslation(const std::vector<Eigen::Vec...
function estimateRelativeTranslationRobust (line 254) | Eigen::Vector3d estimateRelativeTranslationRobust(const std::vector<Eige...
function estimateRelativeTranslationRobust (line 352) | Eigen::Vector3d estimateRelativeTranslationRobust(const std::vector<Eige...
function estimateRelativeMotion (line 460) | void estimateRelativeMotion(const std::vector<Eigen::Vector3d> &pts1,
type SnavelyReprojectionError (line 511) | struct SnavelyReprojectionError {
method SnavelyReprojectionError (line 513) | SnavelyReprojectionError(const double _key_x,const double _key_y,
type SnavelyReprojectionErrorSimple (line 561) | struct SnavelyReprojectionErrorSimple {
method SnavelyReprojectionErrorSimple (line 563) | SnavelyReprojectionErrorSimple(
function setSmallRotationMatrix (line 612) | void setSmallRotationMatrix(const double *w,Eigen::Matrix3d &rotation){
type GSLAM (line 619) | namespace GSLAM{
FILE: GSLAM/Estimation.h
function namespace (line 12) | namespace GSLAM {
FILE: GSLAM/Frame.cpp
type GSLAM (line 12) | namespace GSLAM{
FILE: GSLAM/Frame.h
function namespace (line 35) | namespace GSLAM
FILE: GSLAM/Geometry.cpp
function multiviewTriangulationLinear (line 13) | Eigen::Vector3d multiviewTriangulationLinear(const std::vector<Eigen::Ve...
function multiviewTriangulationDepth (line 66) | double multiviewTriangulationDepth(const std::vector<Eigen::Vector3d> &...
function AngleAxis2RotationMatrix (line 100) | inline Eigen::Matrix3d AngleAxis2RotationMatrix(const double costheta,
function ComputeAxisRotation (line 119) | Eigen::Matrix3d ComputeAxisRotation(const Eigen::Vector3d &v1,const Eige...
FILE: GSLAM/Geometry.h
function setCrossMatrix (line 15) | inline void setCrossMatrix(Eigen::Matrix3d &crossMat,const Eigen::Vector...
FILE: GSLAM/GlobalReconstruction.cpp
type GSLAM (line 24) | namespace GSLAM {
type GlobalError2 (line 451) | struct GlobalError2{
method GlobalError2 (line 453) | GlobalError2(const double _tracked_x,const double _tracked_y):
function myfunction (line 918) | bool myfunction (const LocalMapPoint* p1,const LocalMapPoint* p2) { re...
FILE: GSLAM/GlobalReconstruction.h
function Eigen (line 17) | inline Eigen::Vector3d ComputeRelativeRotationError(const Eigen::Matrix3...
function ApplyRotation (line 28) | inline void ApplyRotation(const Eigen::Vector3d& rotation_change,Eigen::...
function max3 (line 36) | inline int max3(int a, int b, int c){
type ScaleConstraint (line 44) | typedef struct{
type RotationConstraint (line 60) | typedef struct{
type TranslationConstraint (line 76) | typedef struct{
type SIM3Constraint (line 93) | typedef struct{
function class (line 111) | class GlobalScaleEstimation{
function class (line 142) | class GlobalRotationEstimation{
FILE: GSLAM/GlobalRotationEstimation.cpp
type GSLAM (line 15) | namespace GSLAM{
FILE: GSLAM/GlobalScaleEstimation.cpp
type GSLAM (line 13) | namespace GSLAM{
FILE: GSLAM/GlobalTranslationEstimation.cpp
type GSLAM (line 12) | namespace GSLAM{
FILE: GSLAM/Homography.hpp
type GSLAM (line 15) | namespace GSLAM{
function icvGetRTMatrix (line 17) | static void icvGetRTMatrix(const CvPoint2D32f* a,
function EstimateRigidTransform (line 116) | static int EstimateRigidTransform(cv::AutoBuffer<int> &good_idx,
function EstimateRigidTransform (line 280) | static cv::Mat EstimateRigidTransform(cv::AutoBuffer<int> &goodIdx,
function homographyRefineLM (line 298) | static bool homographyRefineLM(const std::vector<cv::Point2d>& pts1,
function EstimateHomography (line 349) | static cv::Mat EstimateHomography(
type GSLAM (line 348) | namespace GSLAM{
function icvGetRTMatrix (line 17) | static void icvGetRTMatrix(const CvPoint2D32f* a,
function EstimateRigidTransform (line 116) | static int EstimateRigidTransform(cv::AutoBuffer<int> &good_idx,
function EstimateRigidTransform (line 280) | static cv::Mat EstimateRigidTransform(cv::AutoBuffer<int> &goodIdx,
function homographyRefineLM (line 298) | static bool homographyRefineLM(const std::vector<cv::Point2d>& pts1,
function EstimateHomography (line 349) | static cv::Mat EstimateHomography(
FILE: GSLAM/IMU.hpp
function setCameraIntrisicMatrix (line 17) | static void setCameraIntrisicMatrix(cv::Mat &K,cv::Mat &invK,const Camer...
function getSkewRotation (line 27) | static cv::Mat getSkewRotation(const cv::Scalar &angular){
type MOTION_DATA (line 45) | struct MOTION_DATA{
class IMU (line 52) | class IMU{
method getDiffRotation (line 60) | void getDiffRotation(cv::Mat &rotation,
method loadImuData (line 87) | void loadImuData(const char* filename){
method updateMotionIndex (line 191) | void updateMotionIndex(const double frameStamp,int &motionIndex){
method synchronization (line 205) | void synchronization(std::vector<double> &frameStamps,double ts){
method getIntraFrameRotation (line 213) | void getIntraFrameRotation(std::vector<cv::Mat> &rotations,
method getInterFrameRotation (line 246) | void getInterFrameRotation(cv::Mat &rotation,int &nxtIndex,const doubl...
FILE: GSLAM/ImageGrids.hpp
type TRIANGLE (line 20) | struct TRIANGLE{
type EDGE (line 25) | struct EDGE{
class ImageGrids (line 29) | class ImageGrids{
method initializeGridVertices (line 38) | void initializeGridVertices(cv::Size gridSize,cv::Size sizeByVertex){
method initialize (line 62) | void initialize(cv::Size _gridSize,cv::Size _sizeByGrid,cv::Size _size...
method rotateAndNormalizePoint (line 73) | void rotateAndNormalizePoint(const cv::Point2f &src,
method prepareMeshByRow (line 110) | void prepareMeshByRow(const std::vector<cv::Mat> &transformArray){
method prepareMeshByCopy (line 130) | void prepareMeshByCopy(){
method release (line 134) | void release(){
FILE: GSLAM/KLT.h
type KLT_TrackingContextRec (line 18) | typedef struct {
type KLT_FeatureRec (line 76) | typedef struct {
type KLT_FeatureListRec (line 100) | typedef struct {
type ConvolutionKernel (line 109) | typedef struct {
type KLT_locType (line 119) | typedef float KLT_locType;
type KLT_PixelType (line 120) | typedef unsigned char KLT_PixelType;
FILE: GSLAM/KLTUtil.cpp
function KLT_FeatureList (line 49) | KLT_FeatureList KLTCreateFeatureList(KLT_TrackingContext tc,int nFeatures)
function KLTChangeTCPyramid (line 87) | void KLTChangeTCPyramid(KLT_TrackingContext tc,
function computeKernels (line 136) | void computeKernels(float sigma,
function _KLTGetKernelWidths (line 207) | void _KLTGetKernelWidths(float sigma,
function _KLTComputeSmoothSigma (line 215) | float _KLTComputeSmoothSigma(KLT_TrackingContext tc){
function _pyramidSigma (line 219) | static float _pyramidSigma(KLT_TrackingContext tc){
function KLTUpdateTCBorder (line 225) | void KLTUpdateTCBorder(KLT_TrackingContext tc){
function KLT_TrackingContext (line 298) | KLT_TrackingContext KLTCreateTrackingContext(){
FILE: GSLAM/KeyFrame.cpp
type GSLAM (line 17) | namespace GSLAM{
FILE: GSLAM/KeyFrame.h
function namespace (line 34) | namespace GSLAM{
FILE: GSLAM/KeyFrameConnection.cpp
type GSLAM (line 12) | namespace GSLAM{
function updateScore (line 14) | void updateScore(ORBVocabulary* mpORBVocabulary,KeyFrame* keyFrame1,Ke...
FILE: GSLAM/KeyFrameConnection.h
function namespace (line 15) | namespace GSLAM {
FILE: GSLAM/KeyFrameDatabase.cpp
type GSLAM (line 30) | namespace GSLAM{
FILE: GSLAM/KeyFrameDatabase.h
function namespace (line 35) | namespace GSLAM
FILE: GSLAM/LK.hpp
function KLTCountRemainingFeatures (line 27) | int KLTCountRemainingFeatures(KLT_FeatureList fl){
function compensatePatchLighting (line 43) | static void compensatePatchLighting(float* patch,
function computeBilinearWeight (line 56) | static void computeBilinearWeight(float x,float y,int &xt,int &yt,float ...
function computeBilinearPatch (line 71) | static void computeBilinearPatch(float* patch,
function computeImageDiff (line 110) | static void computeImageDiff(const float* patch1,const float* patch2,flo...
function computeImageSum (line 118) | static void computeImageSum(const float* patch1,const float* patch2,floa...
function compute2X2GradientMatrix (line 127) | static void compute2X2GradientMatrix(const float* gradx,const float* gra...
function compute2X1ErrorVector (line 148) | static void compute2X1ErrorVector(const float* diff,const float* gradx,c...
function _solveEquation (line 168) | static int _solveEquation(float gxx, float gxy, float gyy,
function _outOfBounds (line 185) | static bool _outOfBounds(float x,float y,
function computeABSImageDiff (line 193) | static float computeABSImageDiff(float *patchdiff,const int patchsize){
function _interpolate (line 227) | static float _interpolate(float x,
function _computeIntensityDifference (line 254) | static void _computeIntensityDifference(
function _computeGradientSum (line 285) | static void _computeGradientSum(
function _compute2by2GradientMatrix (line 318) | static void _compute2by2GradientMatrix(
function _compute2by1ErrorVector (line 348) | static void _compute2by1ErrorVector(
function _FloatWindow (line 379) | static _FloatWindow _allocateFloatWindow(
function _sumAbsFloatWindow (line 392) | static float _sumAbsFloatWindow(
function _trackFeature (line 413) | static int _trackFeature(float x1, /* location of window in first image */
function refineFeatureTranslation (line 671) | static int refineFeatureTranslation(float *x2,
class KLTInvoker (line 798) | class KLTInvoker{
method KLTInvoker (line 809) | KLTInvoker(std::vector<cv::Mat>* _pyramid1,
function KLTTrackFeatures (line 972) | void KLTTrackFeatures(KLT_TrackingContext tc,
function compensateLightingAndMotion (line 1048) | void compensateLightingAndMotion(const std::vector<cv::Point2f>& pts1,
FILE: GSLAM/MapPoint.cpp
type GSLAM (line 12) | namespace GSLAM{
FILE: GSLAM/MapPoint.h
function namespace (line 17) | namespace GSLAM{
FILE: GSLAM/ORBVocabulary.h
function namespace (line 27) | namespace GSLAM{
FILE: GSLAM/ORBextractor.cc
type GSLAM (line 69) | namespace GSLAM
function HarrisResponses (line 77) | static void
function IC_Angle (line 120) | static float IC_Angle(const Mat& image, Point2f pt, const vector<int>...
function computeOrbDescriptor (line 151) | static void computeOrbDescriptor(const KeyPoint& kpt,
function computeOrientation (line 515) | static void computeOrientation(const Mat& image, vector<KeyPoint>& key...
function computeDescriptors (line 1085) | static void computeDescriptors(const Mat& image, vector<KeyPoint>& key...
FILE: GSLAM/ORBextractor.h
function namespace (line 29) | namespace GSLAM
FILE: GSLAM/ORBmatcher.cc
type GSLAM (line 32) | namespace GSLAM
function errorCompare (line 53) | bool errorCompare(const pair<int,double>& e1, const pair<int,double>& ...
FILE: GSLAM/ORBmatcher.h
function namespace (line 33) | namespace GSLAM
FILE: GSLAM/RelativeMotion.hpp
type RotationTranslation (line 15) | struct RotationTranslation{
method RotationTranslation (line 17) | RotationTranslation(const double _x1,const double _y1,const double _z1,
type Rotation (line 74) | struct Rotation{
method Rotation (line 76) | Rotation(const double _x1,const double _y1,const double _z1,
function estimateRelativeTranslation (line 114) | Eigen::Vector3d estimateRelativeTranslation(const std::vector<Eigen::Vec...
function preSolve (line 134) | void preSolve(const std::vector<Eigen::Vector3d> &pts1,
function estimateRotationTranslation (line 176) | void estimateRotationTranslation(const double lossThreshold,
function estimateRotationTranslation (line 218) | void estimateRotationTranslation(const double lossThreshold,
type Transform (line 263) | struct Transform{
method Transform (line 265) | Transform(const double _x1,const double _y1,const double _z1,
function estimateRotationTranslation2 (line 310) | void estimateRotationTranslation2(const double lossThreshold,
FILE: GSLAM/Settings.h
function namespace (line 16) | namespace GSLAM{
FILE: GSLAM/System.cpp
type GSLAM (line 19) | namespace GSLAM{
function processKeyFrame (line 166) | void processKeyFrame(System& system,KeyFrame& keyFrame){
class NormalizeFrameInvoker (line 223) | class NormalizeFrameInvoker{
method NormalizeFrameInvoker (line 231) | NormalizeFrameInvoker(KeyFrame* _keyFrame,
class NormalizeKeyFrameInvoker (line 269) | class NormalizeKeyFrameInvoker{
method NormalizeKeyFrameInvoker (line 279) | NormalizeKeyFrameInvoker(KeyFrame* _keyFrame,
function Transform (line 323) | Transform System::Track(cv::Mat &im, const double ×tamp,const int...
FILE: GSLAM/System.h
function namespace (line 28) | namespace GSLAM{
FILE: GSLAM/Transform.cpp
type GSLAM (line 13) | namespace GSLAM{
function Transform (line 39) | Transform Transform::leftMultiply(const Transform& transform)const{
function Transform (line 46) | Transform Transform::inverse()const{
FILE: GSLAM/Transform.h
function namespace (line 14) | namespace GSLAM{
FILE: GSLAM/convolve.cpp
function computeGxGy (line 15) | inline void computeGxGy(const cv::Mat& img,cv::Mat& grad,cv::Mat& filter...
function computeGradients (line 19) | void computeGradients(const cv::Mat &img,
function computeSmoothedImage (line 46) | void computeSmoothedImage(const cv::Mat &img,
FILE: GSLAM/error.cpp
function KLTError (line 21) | void KLTError(char *fmt, ...)
function KLTWarning (line 43) | void KLTWarning(char *fmt, ...)
FILE: GSLAM/experiment_helpers.hpp
type opengv (line 37) | namespace opengv
FILE: GSLAM/las2.cpp
type storeVals (line 45) | enum storeVals {STORQ = 1, RETRQ, STORP, RETRP}
function check_parameters (line 220) | long check_parameters(SMat A, long dimensions, long iterations,
function write_header (line 244) | void write_header(long iterations, long dimensions, double endl, double ...
function SVDRec (line 336) | SVDRec svdLAS2A(SMat A, long dimensions) {
function SVDRec (line 347) | SVDRec svdLAS2(SMat A, long dimensions, long iterations, double end[2],
function rotateArray (line 558) | void rotateArray(double *a, int size, int x) {
function ritvec (line 577) | long ritvec(long n, SMat A, SVDRec R, double kappa, double *ritz, double...
function lanso (line 723) | int lanso(SMat A, long iterations, long dimensions, double endl,
function lanczos_step (line 852) | long lanczos_step(SMat A, long first, long last, double *wptr[],
function ortbnd (line 970) | void ortbnd(double *alf, double *eta, double *oldeta, double *bet, long ...
function purge (line 1033) | void purge(long n, long ll, double *r, double *q, double *ra,
function stpone (line 1121) | void stpone(SMat A, double *wrkptr[], double *rnmp, double *tolp, long n) {
function startv (line 1187) | double startv(SMat A, double *wptr[], long step, long n) {
function error_bound (line 1265) | long error_bound(long *enough, double endl, double endr,
function imtqlb (line 1350) | void imtqlb(long n, double d[], double e[], double bnd[])
function imtql2 (line 1506) | void imtql2(long nm, long n, double d[], double e[], double z[])
function machar (line 1664) | void machar(long *ibeta, long *it, long *irnd, long *machep, long *negep) {
function store (line 1770) | void store(long n, long isw, long j, double *s) {
FILE: GSLAM/main.cpp
function main (line 18) | int main(int argc, char **argv){
FILE: GSLAM/pyramid.cpp
function computeGxGy (line 41) | inline void computeGxGy(const cv::Mat& img,cv::Mat& grad,cv::Mat& filter...
function computePyramid (line 45) | void computePyramid(KLT_TrackingContext tc,
FILE: GSLAM/random_generators.hpp
type opengv (line 38) | namespace opengv
FILE: GSLAM/selectGoodFeatures.hpp
function _fillFeaturemap (line 37) | static void _fillFeaturemap(int x, int y,
function _comparePoints (line 70) | static int _comparePoints(const void *a, const void *b)
function _sortPointList (line 80) | static void _sortPointList(
function _enforceMinimumDistance (line 88) | static void _enforceMinimumDistance(uchar *featuremap,
function _minEigenvalue (line 220) | static float _minEigenvalue(float gxx, float gxy, float gyy)
type greaterThanPtr (line 225) | struct greaterThanPtr :
function _KLTSelectGoodFeatures (line 234) | void _KLTSelectGoodFeatures(KLT_TrackingContext tc,
function KLTSelectGoodFeatures (line 424) | void KLTSelectGoodFeatures(KLT_TrackingContext tc,
function KLTReplaceLostFeatures (line 436) | void KLTReplaceLostFeatures(KLT_TrackingContext tc,
function _fillFeaturemap2 (line 454) | static void _fillFeaturemap2(int x, int y,
function _comparePoints2 (line 487) | static int _comparePoints2(const void *a, const void *b)
function _sortPointList2 (line 497) | static void _sortPointList2(
function _minEigenvalue2 (line 503) | static float _minEigenvalue2(float gxx, float gxy, float gyy)
function _enforceMinimumDistance2 (line 507) | void _enforceMinimumDistance2(uchar* featuremap,
function _KLTSelectGoodFeatures (line 620) | void _KLTSelectGoodFeatures(KLT_TrackingContext tc,
function _KLTSelectGoodFeatures2 (line 808) | void _KLTSelectGoodFeatures2(KLT_TrackingContext tc,
function KLTSelectGoodFeatures2 (line 997) | void KLTSelectGoodFeatures2(KLT_TrackingContext tc,
function KLTReplaceLostFeatures2 (line 1009) | void KLTReplaceLostFeatures2(KLT_TrackingContext tc,
type greaterThanPtrInt (line 1026) | struct greaterThanPtrInt:
type smallerDistance (line 1033) | struct smallerDistance:
function initializeGridSequence (line 1055) | void initializeGridSequence(){
function _KLTSelectGoodFeaturesUniform (line 1064) | void _KLTSelectGoodFeaturesUniform(KLT_TrackingContext tc,
function KLTSelectGoodFeaturesUniform (line 1318) | void KLTSelectGoodFeaturesUniform(
function KLTReplaceLostFeaturesUniform (line 1341) | void KLTReplaceLostFeaturesUniform(
FILE: GSLAM/selectGoodFeatures2.hpp
function _fillFeaturemap (line 36) | static void _fillFeaturemap(int x, int y,
function _comparePoints (line 69) | static int _comparePoints(const void *a, const void *b)
function _sortPointList (line 79) | static void _sortPointList(
function _enforceMinimumDistance (line 87) | static void _enforceMinimumDistance(uchar *featuremap,
function _minEigenvalue (line 219) | static float _minEigenvalue(float gxx, float gxy, float gyy)
type greaterThanPtr (line 224) | struct greaterThanPtr :
function _KLTSelectGoodFeatures (line 233) | void _KLTSelectGoodFeatures(KLT_TrackingContext tc,
function KLTSelectGoodFeatures (line 423) | void KLTSelectGoodFeatures(KLT_TrackingContext tc,
function KLTReplaceLostFeatures (line 435) | void KLTReplaceLostFeatures(KLT_TrackingContext tc,
function _fillFeaturemap2 (line 453) | static void _fillFeaturemap2(int x, int y,
function _comparePoints2 (line 486) | static int _comparePoints2(const void *a, const void *b)
function _sortPointList2 (line 496) | static void _sortPointList2(
function _minEigenvalue2 (line 502) | static float _minEigenvalue2(float gxx, float gxy, float gyy)
function _enforceMinimumDistance2 (line 506) | void _enforceMinimumDistance2(uchar* featuremap,
function _KLTSelectGoodFeatures (line 619) | void _KLTSelectGoodFeatures(KLT_TrackingContext tc,
function KLTSelectGoodFeatures2 (line 806) | void KLTSelectGoodFeatures2(KLT_TrackingContext tc,
function KLTReplaceLostFeatures2 (line 818) | void KLTReplaceLostFeatures2(KLT_TrackingContext tc,
type greaterThanPtrInt (line 835) | struct greaterThanPtrInt:
type smallerDistance (line 842) | struct smallerDistance:
function initializeGridSequence (line 864) | void initializeGridSequence(){
function _KLTSelectGoodFeaturesUniform (line 873) | void _KLTSelectGoodFeaturesUniform(KLT_TrackingContext tc,
function KLTSelectGoodFeaturesUniform (line 1127) | void KLTSelectGoodFeaturesUniform(
function KLTReplaceLostFeaturesUniform (line 1150) | void KLTReplaceLostFeaturesUniform(
FILE: GSLAM/svdlib.cpp
function svdResetCounters (line 41) | void svdResetCounters(void) {
function DMat (line 51) | DMat svdNewDMat(int rows, int cols) {
function svdFreeDMat (line 68) | void svdFreeDMat(DMat D) {
function SMat (line 76) | SMat svdNewSMat(int rows, int cols, int vals) {
function svdFreeSMat (line 91) | void svdFreeSMat(SMat S) {
function SVDRec (line 101) | SVDRec svdNewSVDRec(void) {
function svdFreeSVDRec (line 108) | void svdFreeSVDRec(SVDRec R) {
function DMat (line 120) | DMat svdConvertStoD(SMat S) {
function SMat (line 135) | SMat svdConvertDtoS(DMat D) {
function DMat (line 161) | DMat svdTransposeD(DMat D) {
function SMat (line 171) | SMat svdTransposeS(SMat S) {
function svdWriteDenseArray (line 197) | void svdWriteDenseArray(double *a, int n, char *filename, char binary) {
function SMat (line 255) | static SMat svdLoadSparseTextHBFile(FILE *file) {
function svdWriteSparseTextHBFile (line 306) | static void svdWriteSparseTextHBFile(SMat S, FILE *file) {
function SMat (line 333) | static SMat svdLoadSparseTextFile(FILE *file) {
function svdWriteSparseTextFile (line 361) | static void svdWriteSparseTextFile(SMat S, FILE *file) {
function SMat (line 372) | static SMat svdLoadSparseBinaryFile(FILE *file) {
function svdWriteSparseBinaryFile (line 408) | static void svdWriteSparseBinaryFile(SMat S, FILE *file) {
function DMat (line 423) | static DMat svdLoadDenseTextFile(FILE *file) {
function svdWriteDenseTextFile (line 444) | static void svdWriteDenseTextFile(DMat D, FILE *file) {
function DMat (line 453) | static DMat svdLoadDenseBinaryFile(FILE *file) {
function svdWriteDenseBinaryFile (line 478) | static void svdWriteDenseBinaryFile(DMat D, FILE *file) {
function SMat (line 488) | SMat svdLoadSparseMatrix(char *filename, int format) {
function DMat (line 518) | DMat svdLoadDenseMatrix(char *filename, int format) {
function svdWriteSparseMatrix (line 548) | void svdWriteSparseMatrix(SMat S, char *filename, int format) {
function svdWriteDenseMatrix (line 579) | void svdWriteDenseMatrix(DMat D, char *filename, int format) {
FILE: GSLAM/svdlib.hpp
type smat (line 43) | struct smat
type dmat (line 44) | struct dmat
type svdrec (line 45) | struct svdrec
type smat (line 52) | struct smat {
type dmat (line 62) | struct dmat {
type svdrec (line 68) | struct svdrec {
type svdCounters (line 87) | enum svdCounters {SVD_MXV, SVD_COUNTERS}
type svdFileFormats (line 91) | enum svdFileFormats {SVD_F_STH, SVD_F_ST, SVD_F_SB, SVD_F_DT, SVD_F_DB}
FILE: GSLAM/svdutil.cpp
function svd_beep (line 77) | void svd_beep(void) {
function svd_debug (line 82) | void svd_debug(char *fmt, ...) {
function svd_error (line 89) | void svd_error(char *fmt, ...) {
function svd_fatalError (line 99) | void svd_fatalError(char *fmt, ...) {
function registerPipe (line 110) | static void registerPipe(FILE *p) {
function isPipe (line 115) | static char isPipe(FILE *p) {
function FILE (line 123) | static FILE *openPipe(char *pipeName, char *mode) {
function FILE (line 130) | static FILE *readZippedFile(char *command, char *fileName) {
function FILE (line 136) | FILE *svd_fatalReadFile(char *filename) {
function stringEndsIn (line 143) | static int stringEndsIn(char *s, char *t) {
function FILE (line 151) | FILE *svd_readFile(char *fileName) {
function FILE (line 198) | static FILE *writeZippedFile(char *fileName, char append) {
function FILE (line 210) | FILE *svd_writeFile(char *fileName, char append) {
function svd_closeFile (line 227) | void svd_closeFile(FILE *file) {
function svd_readBinInt (line 234) | char svd_readBinInt(FILE *file, int *val) {
function svd_readBinFloat (line 244) | char svd_readBinFloat(FILE *file, float *val) {
function svd_writeBinInt (line 256) | char svd_writeBinInt(FILE *file, int x) {
function svd_writeBinFloat (line 263) | char svd_writeBinFloat(FILE *file, float r) {
function svd_fsign (line 274) | double svd_fsign(double a, double b) {
function svd_dmax (line 282) | double svd_dmax(double a, double b) {
function svd_dmin (line 289) | double svd_dmin(double a, double b) {
function svd_imax (line 296) | long svd_imax(long a, long b) {
function svd_imin (line 303) | long svd_imin(long a, long b) {
function svd_dscal (line 311) | void svd_dscal(long n, double da, double *dx, long incx) {
function svd_datx (line 333) | void svd_datx(long n, double da, double *dx, long incx, double *dy, long...
function svd_dcopy (line 362) | void svd_dcopy(long n, double *dx, long incx, double *dy, long incy) {
function svd_ddot (line 388) | double svd_ddot(long n, double *dx, long incx, double *dy, long incy) {
function svd_daxpy (line 420) | void svd_daxpy (long n, double da, double *dx, long incx, double *dy, lo...
function svd_dsort2 (line 447) | void svd_dsort2(long igap, long n, double *array1, double *array2) {
function svd_dswap (line 476) | void svd_dswap(long n, double *dx, long incx, double *dy, long incy) {
function svd_idamax (line 509) | long svd_idamax(long n, double *dx, long incx) {
function svd_opb (line 544) | void svd_opb(SMat A, double *x, double *y, double *temp) {
function svd_opa (line 573) | void svd_opa(SMat A, double *x, double *y) {
function svd_random2 (line 620) | double svd_random2(long *iy) {
function svd_pythag (line 672) | double svd_pythag(double a, double b) {
FILE: GSLAM/time_measurement.hpp
type timeval (line 38) | struct timeval {
type timeval (line 51) | struct timeval
type timeval (line 51) | struct timeval
Condensed preview — 71 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (797K chars).
[
{
"path": "GSLAM/Drawer copy.h",
"chars": 9417,
"preview": "//\n// Drawer.h\n// GSLAM\n//\n// Created by ctang on 11/19/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifn"
},
{
"path": "GSLAM/Drawer.cpp",
"chars": 120,
"preview": "//\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",
"chars": 13389,
"preview": "//\n// Drawer.h\n// GSLAM\n//\n// Created by ctang on 11/19/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifn"
},
{
"path": "GSLAM/Estimation.cpp",
"chars": 69909,
"preview": "//\n// Estimation.cpp\n// GSLAM\n//\n// Created by ctang on 9/10/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n"
},
{
"path": "GSLAM/Estimation.h",
"chars": 2432,
"preview": "//\n// Optimization.h\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n"
},
{
"path": "GSLAM/Frame.cpp",
"chars": 8513,
"preview": "//\n// Frame.cpp\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#incl"
},
{
"path": "GSLAM/Frame.h",
"chars": 3346,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/Geometry.cpp",
"chars": 4302,
"preview": "//\n// Geometry.cpp\n// GSLAM\n//\n// Created by ctang on 9/17/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#"
},
{
"path": "GSLAM/Geometry.h",
"chars": 1869,
"preview": "//\n// Geometry.hpp\n// GSLAM\n//\n// Created by ctang on 9/17/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#"
},
{
"path": "GSLAM/GlobalReconstruction.cpp",
"chars": 45112,
"preview": "\n//\n// GlobalReconstruction.cpp\n// GSLAM\n//\n// Created by ctang on 9/28/16.\n// Copyright © 2016 ctang. All rights re"
},
{
"path": "GSLAM/GlobalReconstruction.h",
"chars": 7040,
"preview": "//\n// GlobalReconstruction.h\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserv"
},
{
"path": "GSLAM/GlobalRotationEstimation.cpp",
"chars": 12829,
"preview": "//\n// GlobalRotationEstimation.cpp\n// GSLAM\n//\n// Created by ctang on 10/2/16.\n// Copyright © 2016 ctang. All rights"
},
{
"path": "GSLAM/GlobalScaleEstimation.cpp",
"chars": 9587,
"preview": "//\n// GlobalScaleEstimation.cpp\n// GSLAM\n//\n// Created by ctang on 9/29/16.\n// Copyright © 2016 ctang. All rights re"
},
{
"path": "GSLAM/GlobalTranslationEstimation.cpp",
"chars": 8411,
"preview": "//\n// GlobalTranslationEstimation.cpp\n// GSLAM\n//\n// Created by ctang on 10/3/16.\n// Copyright © 2016 ctang. All rig"
},
{
"path": "GSLAM/Homography.hpp",
"chars": 13149,
"preview": "//\n// Homography.hpp\n// GSLAM\n//\n// Created by Chaos on 2017-02-09.\n// Copyright © 2017 ctang. All rights reserved.\n"
},
{
"path": "GSLAM/IMU.hpp",
"chars": 7699,
"preview": "//\n// IMU.hpp\n// STARCK\n//\n// Created by Chaos on 4/6/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n#includ"
},
{
"path": "GSLAM/ImageGrids.hpp",
"chars": 4066,
"preview": "//\n// ImageGrids.hpp\n// STARCK\n//\n// Created by Chaos on 4/7/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n"
},
{
"path": "GSLAM/KLT.h",
"chars": 3814,
"preview": "//\n// KLTCommon.hpp\n// STARCK\n//\n// Created by Chaos on 3/31/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n"
},
{
"path": "GSLAM/KLTUtil.cpp",
"chars": 11134,
"preview": "//\n// KLT.cpp\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#includ"
},
{
"path": "GSLAM/KLTUtil.h",
"chars": 421,
"preview": "//\n// KLTUtil.cpp\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#in"
},
{
"path": "GSLAM/KeyFrame.cpp",
"chars": 16555,
"preview": "//\n// KeyFrame.cpp\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#i"
},
{
"path": "GSLAM/KeyFrame.h",
"chars": 4110,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/KeyFrameConnection.cpp",
"chars": 26363,
"preview": "//\n// KeyFrameConnection.cpp\n// GSLAM\n//\n// Created by ctang on 9/24/16.\n// Copyright © 2016 ctang. All rights reser"
},
{
"path": "GSLAM/KeyFrameConnection.h",
"chars": 1236,
"preview": "//\n// KeyFrameConnection.hpp\n// GSLAM\n//\n// Created by ctang on 9/24/16.\n// Copyright © 2016 ctang. All rights reser"
},
{
"path": "GSLAM/KeyFrameDatabase.cpp",
"chars": 9729,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/KeyFrameDatabase.h",
"chars": 1637,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/L1Solver.h",
"chars": 335,
"preview": "//\n// L1Solver.h\n// GSLAM\n//\n// Created by ctang on 10/2/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#if"
},
{
"path": "GSLAM/LK.hpp",
"chars": 39253,
"preview": "//\n// trackFeatures.hpp\n// STARCK\n//\n// Created by Chaos on 4/1/16.\n// Copyright © 2016 Chaos. All rights reserved.\n"
},
{
"path": "GSLAM/MapPoint.cpp",
"chars": 188,
"preview": "//\n// MapPoint.cpp\n// GSLAM\n//\n// Created by ctang on 9/20/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#"
},
{
"path": "GSLAM/MapPoint.h",
"chars": 1893,
"preview": "//\n// Header.h\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n#ifndef"
},
{
"path": "GSLAM/ORBVocabulary.h",
"chars": 1062,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/ORBextractor.cc",
"chars": 46121,
"preview": "/**\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 "
},
{
"path": "GSLAM/ORBextractor.h",
"chars": 3481,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/ORBmatcher.cc",
"chars": 34729,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/ORBmatcher.h",
"chars": 3351,
"preview": "/**\n* This file is part of ORB-SLAM2.\n*\n* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University "
},
{
"path": "GSLAM/RelativeMotion.hpp",
"chars": 12265,
"preview": "//\n// RelativeMotion.hpp\n// GSLAM\n//\n// Created by Chaos on 2/17/17.\n// Copyright © 2017 ctang. All rights reserved."
},
{
"path": "GSLAM/Settings.h",
"chars": 1094,
"preview": "//\n// Common.h\n// GSLAM\n//\n// Created by ctang on 9/4/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#ifnde"
},
{
"path": "GSLAM/System.cpp",
"chars": 34866,
"preview": "//\n// System.cpp\n// GSLAM\n//\n// Created by ctang on 9/3/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#inc"
},
{
"path": "GSLAM/System.h",
"chars": 3498,
"preview": "//\n// System.h\n// GSLAM\n//\n// Created by ctang on 8/30/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#incl"
},
{
"path": "GSLAM/TrackUtil.cpp",
"chars": 144,
"preview": "//\n// TrackUtil.cpp\n// GSLAM\n//\n// Created by ctang on 9/10/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n"
},
{
"path": "GSLAM/TrackUtil.h",
"chars": 137,
"preview": "//\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#"
},
{
"path": "GSLAM/Transform.cpp",
"chars": 1209,
"preview": "//\n// Transform.cpp\n// GSLAM\n//\n// Created by ctang on 9/5/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#"
},
{
"path": "GSLAM/Transform.h",
"chars": 687,
"preview": "//\n// CameraPose.h\n// GSLAM\n//\n// Created by ctang on 9/5/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#i"
},
{
"path": "GSLAM/convolve.cpp",
"chars": 2913,
"preview": "//\n// convolve.cpp\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#i"
},
{
"path": "GSLAM/convolve.h",
"chars": 499,
"preview": "//\n// convolve.hpp\n// STARCK\n//\n// Created by Chaos on 3/29/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n"
},
{
"path": "GSLAM/error.cpp",
"chars": 1016,
"preview": "/*********************************************************************\n * error.c\n *\n * Error and warning messages, and "
},
{
"path": "GSLAM/error.h",
"chars": 262,
"preview": "/*********************************************************************\n * error.h\n *************************************"
},
{
"path": "GSLAM/experiment_helpers.hpp",
"chars": 5887,
"preview": "/******************************************************************************\n * Author: Laurent Kneip "
},
{
"path": "GSLAM/las2.cpp",
"chars": 59135,
"preview": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source"
},
{
"path": "GSLAM/main.cpp",
"chars": 4718,
"preview": "//\n// main.cpp\n// STAR\n//\n// Created by Chaos on 4/25/16.\n// Copyright © 2016 Chaos. All rights reserved.\n//\n\n#inclu"
},
{
"path": "GSLAM/pyramid.cpp",
"chars": 3006,
"preview": "//\n// pyramid.cpp\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n\n#in"
},
{
"path": "GSLAM/pyramid.h",
"chars": 270,
"preview": "//\n// pyramid.h\n// GSLAM\n//\n// Created by ctang on 9/8/16.\n// Copyright © 2016 ctang. All rights reserved.\n//\n#inclu"
},
{
"path": "GSLAM/random_generators.hpp",
"chars": 2998,
"preview": "/******************************************************************************\n * Author: Laurent Kneip "
},
{
"path": "GSLAM/selectGoodFeatures.hpp",
"chars": 45522,
"preview": "//\n// selectGoodFeatures.hpp\n// STARCK\n//\n// Created by Chaos on 3/30/16.\n// Copyright © 2016 Chaos. All rights rese"
},
{
"path": "GSLAM/selectGoodFeatures2.hpp",
"chars": 39021,
"preview": "//\n// selectGoodFeatures.hpp\n// STARCK\n//\n// Created by Chaos on 3/30/16.\n// Copyright © 2016 Chaos. All rights rese"
},
{
"path": "GSLAM/svdlib.cpp",
"chars": 16780,
"preview": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source"
},
{
"path": "GSLAM/svdlib.hpp",
"chars": 5461,
"preview": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source"
},
{
"path": "GSLAM/svdutil.cpp",
"chars": 19414,
"preview": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source"
},
{
"path": "GSLAM/svdutil.hpp",
"chars": 7664,
"preview": "/*\nCopyright © 2002, University of Tennessee Research Foundation.\nAll rights reserved.\n\nRedistribution and use in source"
},
{
"path": "GSLAM/time_measurement.hpp",
"chars": 2830,
"preview": "/******************************************************************************\r\n * Author: Laurent Kneip "
},
{
"path": "GSLAM.xcodeproj/project.pbxproj",
"chars": 31968,
"preview": "// !$*UTF8*$!\n{\n\tarchiveVersion = 1;\n\tclasses = {\n\t};\n\tobjectVersion = 46;\n\tobjects = {\n\n/* Begin PBXBuildFile section *"
},
{
"path": "GSLAM.xcodeproj/project.xcworkspace/contents.xcworkspacedata",
"chars": 150,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<Workspace\n version = \"1.0\">\n <FileRef\n location = \"self:GSLAM.xcodeproj"
},
{
"path": "GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist",
"chars": 6145,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<Bucket\n type = \"1\"\n version = \"2.0\">\n <Breakpoints>\n <BreakpointProxy"
},
{
"path": "GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcschemes/GSLAM.xcscheme",
"chars": 3265,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<Scheme\n LastUpgradeVersion = \"0720\"\n version = \"1.3\">\n <BuildAction\n "
},
{
"path": "GSLAM.xcodeproj/xcuserdata/chaos.xcuserdatad/xcschemes/xcschememanagement.plist",
"chars": 477,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<!DOCTYPE plist PUBLIC \"-//Apple//DTD PLIST 1.0//EN\" \"http://www.apple.com/DTDs/P"
},
{
"path": "GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcdebugger/Breakpoints_v2.xcbkptlist",
"chars": 20279,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<Bucket\n type = \"1\"\n version = \"2.0\">\n <Breakpoints>\n <BreakpointProxy"
},
{
"path": "GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcschemes/GSLAM.xcscheme",
"chars": 3265,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<Scheme\n LastUpgradeVersion = \"0730\"\n version = \"1.3\">\n <BuildAction\n "
},
{
"path": "GSLAM.xcodeproj/xcuserdata/ctang.xcuserdatad/xcschemes/xcschememanagement.plist",
"chars": 477,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<!DOCTYPE plist PUBLIC \"-//Apple//DTD PLIST 1.0//EN\" \"http://www.apple.com/DTDs/P"
},
{
"path": "README.md",
"chars": 2837,
"preview": "# GSLAM: Initialization-robust Monocular Visual SLAM via Global Structure-from-Motion\n\nFor more information see\n[https:/"
}
]
// ... and 2 more files (download for full content)
About this extraction
This page contains the full source code of the frobelbest/GSLAM GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 71 files (748.8 KB), approximately 198.6k tokens, and a symbol index with 296 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.