Skip to content

Commit

Permalink
Keyframes v1
Browse files Browse the repository at this point in the history
  • Loading branch information
@Lujano
Lujano committed Feb 20, 2019
1 parent bc39554 commit 2c35a6b
Show file tree
Hide file tree
Showing 13 changed files with 321 additions and 200 deletions.
4 changes: 2 additions & 2 deletions calibration/calibrationEUROC.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,10 @@
<use_gt type_id="integer">1</use_gt>
<use_ros type_id="integer">1</use_ros>

<num_cells type_id="integer"> 44</num_cells>
<num_cells type_id="integer"> 38</num_cells>
<length_patch type_id="integer"> 3</length_patch>

<detector type_id="integer">0</detector>
<detector type_id="integer">1</detector>
<matcher type_id="integer">0</matcher>
<!--USE_KAZE, 0
USE_AKAZE, 1
Expand Down
9 changes: 5 additions & 4 deletions include/Imu.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ class ImuFilterNode{
void detectAngBias(); // Detectar medidas para la calibración ang
void detectAccBias(); // Detectar medidas para la calibración acc
void printStatistics();
void clearData(); // Borra los datos obtenidos
Point3d transform2World(Point3d acc, Point3d localAngles);
Point3d angularPosition;
Point3d angularVelocity;
Expand All @@ -78,11 +79,11 @@ class ImuFilterNode{
vector <Point3d> rpyAnglesWorld; // orientacion del robot en rpy respecto al mundo
vector <Point3d> accelerationWorld; // aceleracion del robot respecto al mundo

Mat init_rotationMatrix;
Mat final_rotationMatrix;
Mat residual_rotationMatrix;
Matx33f init_rotationMatrix;
Matx33f final_rotationMatrix;
Matx33f residual_rotationMatrix;

vector <Mat> world2imuRotation;
vector <Matx33f> world2imuRotation;

// Residuales
Point3d residualRPY;
Expand Down
4 changes: 2 additions & 2 deletions include/Plus.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ Mat point2Mat(Point3d point);
Point3d Mat2point(Mat position);
Point3d transformationMatrix2RPY(Mat transformationMatrix);
Point3d transformationMatrix2position(Mat transformationMatrix);
Point3d rotationMatrix2RPY(Mat rotationMatrix);
Mat RPY2rotationMatrix(Point3d rpy );
Point3d rotationMatrix2RPY(Matx33f rotationMatrix);
Matx33f RPY2rotationMatrix(Point3d rpy );
Mat RPYAndPosition2transformationMatrix(Point3d rpy, Point3d position);
Mat transformationMatrix2rotationMatrix(Mat transformationMatrix );

Expand Down
17 changes: 12 additions & 5 deletions include/VISystem.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,8 @@ class VISystem
void EstimatePoseFeatures(Frame* _previous_frame, Frame* _current_frame);
void EstimatePoseFeaturesRansac(Frame* _previous_frame, Frame* _current_frame);
void CalculateROI(Mat image);
void AddFrame(Mat _currentImage, vector <Point3d> _imuAngularVelocity, vector <Point3d> _imuAcceleration);
void AddFrame(Mat _currentImage, vector <Point3d> _imuAngularVelocity, vector <Point3d> _imuAcceleration, Point3d _gtPosition) ;
bool AddFrame(Mat _currentImage, vector <Point3d> _imuAngularVelocity, vector <Point3d> _imuAcceleration);
bool AddFrame(Mat _currentImage, vector <Point3d> _imuAngularVelocity, vector <Point3d> _imuAcceleration, Point3d _gtPosition) ;
void ObtainKeypointsTransformation(Mat candidates);
void MatPoint2Keypoints( Mat _MatPoints, vector<KeyPoint> &_outputKeypoints);
void FreeLastFrame();
Expand All @@ -58,6 +58,7 @@ class VISystem
void WarpFunctionRT(vector <KeyPoint> inPoints, Mat rotationMat, Mat translationMat, vector <KeyPoint> &outPoints);
float MedianAbsoluteDeviation(Mat _input);
float MedianMat(Mat _input) ;
float Disparity(vector <KeyPoint> keyPoints, vector <KeyPoint> inPoints );

void setGtRes(Mat TraslationResGT , Mat RotationGT);
Mat getProjectionMat(Mat cameraMat, Mat rotationMat, Mat translationMat);
Expand All @@ -78,7 +79,7 @@ class VISystem
Quaterniond qOrientationImu;
Point3d RPYOrientationImu;
Mat world2imuTransformation;
Mat world2imuRotation;
Matx33f world2imuRotation;

Point3d positionCam;
Point3d velocityCam;
Expand All @@ -90,7 +91,7 @@ class VISystem


Mat imu2camTransformation;
Mat imu2camRotation;
Matx33f imu2camRotation;
Point3d imu2camTranslation;

SE3 final_poseCam;
Expand Down Expand Up @@ -133,8 +134,14 @@ class VISystem
vector<Mat> K_ = vector<Mat>(PYRAMID_LEVELS);
vector<double> Prof;
Mat TraslationResidual;
Mat RotationResidual;
Matx33f RotationResidual;
Matx33f RotationResCam; // residual de rotacion
Matx33f init_rotationMatrix, final_rotationMatrix;
Point3f translationResEst;
int nPointsLastKeyframe;
int nPointsCurrentImage;
bool lastImageWasKeyframe;
bool currentImageIsKeyframe;



Expand Down
6 changes: 3 additions & 3 deletions launch/vi_slam.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@
<launch>
<!-- VI-SLAM main thread-->
<node pkg="vi_slam" type="vi_slam" name="main"
args="-gtFile=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_03_difficult/mav0/state_groundtruth_estimate0/data.csv
-imuFile=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_03_difficult/mav0/imu0/data.csv
-imagesPath=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_03_difficult/mav0/cam0/data/
args="-gtFile=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_01_easy/mav0/state_groundtruth_estimate0/data.csv
-imuFile=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_01_easy/mav0/imu0/data.csv
-imagesPath=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_01_easy/mav0/cam0/data/
-calibrationFile=/home/lujano/catkin_ws/src/vi-slam/calibration/calibrationEUROC.xml
-outputFile=/home/lujano/Documents/outputVISlam.csv"
clear_params="true"
Expand Down
25 changes: 15 additions & 10 deletions src/Camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,41 +93,44 @@ int Camera::detectAndComputeFeatures(){

void Camera::setDetector(int _detector)
{
switch (_detector)
switch (_detector) // Añadir brisk para futuras versiones
{
case USE_KAZE:
{
detector = KAZE::create(); // Normaliza distancia
detector = KAZE::create( false, false, 0.005f); // Normaliza distancia
//detector.setThreshold(200.0);
cout << "Using Kaze detector"<<endl;
break;
}
case USE_AKAZE:
{
detector = AKAZE::create();
detector = AKAZE::create(AKAZE::DESCRIPTOR_MLDB,
0, 3,
0.004f);
cout << "Using Akaze detector"<<endl;
break;
}
case USE_SIFT:
{
detector = SIFT::create();
detector = SIFT::create(200);
cout << "Using SIFT detector"<<endl;
break;
}
case USE_SURF:
{
detector = SURF::create(); // Normaliza distancia
detector = SURF::create(1000); // setThreshold
cout << "Using SURF detector"<<endl;
break;
}
case USE_ORB:
{
detector = ORB::create(600);
detector = ORB::create(200);
cout << "Using ORB detector"<<endl;
break;
}
default:
{
detector = KAZE::create();
detector = KAZE::create(false, false, 0.005);
cout << "Using Kaze detector"<<endl;
break;
}
Expand Down Expand Up @@ -200,17 +203,19 @@ bool Camera::addKeyframe()
cdetect = clock();


if ( (nPointsDetect > 1) && (frameList.size()!= 0))
if ( (nPointsDetect > 10) && (frameList.size()!= 0)) // Frames disferentes al primero
{ // is a keyframe (maybe)
//computeDescriptors();
num_images = num_images+1;
cdescriptors = clock();
computeGoodMatches();
cgood = clock();
computeGradient();
//computeGradient();
cgradient = clock();
/*
ObtainPatchesPointsPreviousFrame();
ObtainDebugPointsPreviousFrame(); // Debug
*/
//computePatches();
//computeResiduals();
cpatches = clock();
Expand All @@ -222,7 +227,7 @@ bool Camera::addKeyframe()
//computeDescriptors();
cdescriptors = clock();
cgood = clock();
computeGradient();
//computeGradient();
cgradient = clock();
cpatches = clock();
saveFrame();
Expand Down
8 changes: 4 additions & 4 deletions src/DataReader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ void DataReader::UpdateDataReader(int index, int index2){
double timeImage = imageReader.getImageTime(imageIndex0+index); // poner en clase
double timeLineImu = imuReader.getGroundTruthData(indexImuReader, 0);
double timeLineGt= gtReader.getGroundTruthData(indexGtReader, 0);
cout << "time dif " << timeImage -timeLineGt <<endl;
//cout << "time dif " << timeImage -timeLineGt <<endl;

double delta1;
double delta2;
Expand All @@ -131,7 +131,7 @@ void DataReader::UpdateDataReader(int index, int index2){
timeLineGt= gtReader.getGroundTruthData(indexGtReader, 0);
delta1 = timeLineGt-timeImage;
}
cout << "time dif " << timeImage -timeLineGt <<endl;
//cout << "time dif " << timeImage -timeLineGt <<endl;
// Encontrar el indice del dato de imu posterior a la imagen1

delta2 = timeLineImu-timeImage;
Expand Down Expand Up @@ -161,8 +161,8 @@ void DataReader::UpdateDataReader(int index, int index2){
accBias.clear();
// Obtener todos los datos entre las dos imagenes
timeImuReader = imuReader.getGroundTruthData(indexImuReader, 0);
cout << "time image 2 = " << imageReader.getImageTime(imageIndex0 +index2) - imageReader.getImageTime(imageIndex0 +index)<<endl;
cout << "time imu reader" << imageReader.getImageTime(imageIndex0 +index)-timeImuReader<<endl;
//cout << "time image 2 = " << imageReader.getImageTime(imageIndex0 +index2) - imageReader.getImageTime(imageIndex0 +index)<<endl;
// cout << "time imu reader" << imageReader.getImageTime(imageIndex0 +index)-timeImuReader<<endl;
while(timeImuReader<=static_cast<double>(imageReader.getImageTime(imageIndex0 +index2)))
{
angularVelocity.x = imuReader.getGroundTruthData(indexImuReader, 1);
Expand Down
39 changes: 22 additions & 17 deletions src/Imu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,20 +124,20 @@ void Imu::calibrateAng(int axis)

void Imu::calibrateAcc(int axis)
{
Point3d gravityinWorld ;
Point3f gravityinWorld ;
gravityinWorld.x = 0.0;
gravityinWorld.y = 0.0;
gravityinWorld.z = 9.68;

Point3d gravityInImu;
Point3d accSum;
Point3f gravityInImu;
Point3f accSum;
accSum.x= 0.0;
accSum.y = 0.0;
accSum.z = 0.0;
for (int i = 0; i < n ; i++)
{
gravityInImu = Mat2point( world2imuRotation[i].t()*point2Mat(gravityinWorld));
accSum = accSum + accelerationMeasure[i] -gravityInImu;
gravityInImu = world2imuRotation[i].t()*gravityinWorld;
accSum = accSum + Point3f(accelerationMeasure[i]) -gravityInImu;
}

if(axis == 0)
Expand Down Expand Up @@ -213,21 +213,21 @@ void Imu::detectAngBias()

void Imu::detectAccBias()
{
Point3d counter = Point3d(0.0, 0.0, 0.0);
Point3d accThreshold = Point3d(0.3, 0.3, 0.1);
Point3f counter = Point3d(0.0, 0.0, 0.0);
Point3f accThreshold = Point3d(0.3, 0.3, 0.1);


Point3d gravityinWorld ;
Point3f gravityinWorld ;
gravityinWorld.x = 0.0;
gravityinWorld.y = 0.0;
gravityinWorld.z = 9.68;

Point3d gravityInImu;
Point3d accMed;
Point3f gravityInImu;
Point3f accMed;
for (int i = 0; i < n ; i++)
{
gravityInImu = Mat2point( world2imuRotation[i].t()*point2Mat(gravityinWorld));
accMed = accelerationMeasure[i] -gravityInImu;
gravityInImu = world2imuRotation[i].t()*gravityinWorld;
accMed = Point3f(accelerationMeasure[i]) -gravityInImu;
if(abs(accMed.x)<accThreshold.x)
{
counter.x++;
Expand Down Expand Up @@ -278,11 +278,8 @@ void Imu::detectAccBias()

void Imu::estimateOrientation()
{
rpyAnglesWorld.clear();
quaternionWorld.clear();
angularVelocityIMUFilter.clear();
world2imuRotation.clear();

clearData();
Quaterniond orientation;
elapsed_filter = 0.0;
for (int i = 0; i < n; i++)
Expand Down Expand Up @@ -319,6 +316,14 @@ void Imu::estimateOrientation()
elapsed_filter = elapsed_filter/n;
}

void Imu:: clearData()
{
rpyAnglesWorld.clear();
quaternionWorld.clear();
angularVelocityIMUFilter.clear();
world2imuRotation.clear();
}

void Imu::computeAcceleration() // Implementar la estimación del bias y ruido
{
accelerationWorld.clear();
Expand All @@ -328,7 +333,7 @@ void Imu::computeAcceleration() // Implementar la estimación del bias y ruido
// restar bias local
accelerationMeasure[i] = accelerationMeasure[i]+accBias;
// transformar acelerarion al sistema fijo (world)
accWorld = Mat2point(world2imuRotation[i]*point2Mat(accelerationMeasure[i]));
accWorld = world2imuRotation[i]*Point3f(accelerationMeasure[i]);
accWorld.z = accWorld.z-9.68; // restar la aceleracion de gravedad
accelerationWorld.push_back(accWorld);
}
Expand Down
2 changes: 2 additions & 0 deletions src/Matcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -294,6 +294,8 @@ void Matcher::getMatches(vector<KeyPoint> &_matched1, vector<KeyPoint> &_matched

void Matcher::getGoodMatches(vector<KeyPoint> &_matched1, vector<KeyPoint> &_matched2)
{
_matched1.clear(); // Borrar datos antiguos
_matched2.clear(); // Borrar datos antiguos
for(unsigned i = 0; i < goodMatches.size(); i++) {
_matched1.push_back(keypoints_1[goodMatches[i].queryIdx]);
_matched2.push_back(keypoints_2[goodMatches[i].trainIdx]);
Expand Down
43 changes: 21 additions & 22 deletions src/Plus.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,22 +53,21 @@ Point3d toRPY(const Quaterniond& q)
return angles;
}

Point3d rotationMatrix2RPY(Mat rotationMatrix)
Point3d rotationMatrix2RPY(Matx33f rotationMatrix)
{
// roll (x-axis rotation)
double roll, pitch, yaw;

double r11 = rotationMatrix.at<float>(0,0);
double r12 = rotationMatrix.at<float>(0,1);
double r13 = rotationMatrix.at<float>(0,2);
double r11 = rotationMatrix(0,0);
double r12 = rotationMatrix(0,1);
double r13 = rotationMatrix(0,2);

double r21 = rotationMatrix.at<float>(1,0);
double r22 = rotationMatrix.at<float>(1,1);
double r23 = rotationMatrix.at<float>(1,2);
double r21 = rotationMatrix(1,0);
double r22 = rotationMatrix(1,1);
double r23 = rotationMatrix(1,2);

double r31 = rotationMatrix.at<float>(2,0);
double r32 = rotationMatrix.at<float>(2,1);
double r33 = rotationMatrix.at<float>(2,2);
double r31 = rotationMatrix(2,0);
double r32 = rotationMatrix(2,1);
double r33 = rotationMatrix(2,2);

yaw = atan2(r21, r11);
pitch = atan2(-r31, sqrt(r32*r32+r33*r33));
Expand Down Expand Up @@ -180,7 +179,7 @@ Point3d toRPY360(Point3d angles)
}


Mat RPY2rotationMatrix(Point3d rpy )
Matx33f RPY2rotationMatrix(Point3d rpy )
{
// roll (x-axis rotation)
double roll = rpy.x;
Expand All @@ -195,20 +194,20 @@ Mat RPY2rotationMatrix(Point3d rpy )
double c3 = cos(yaw);
double s3 = sin(yaw);

Mat rotationMatrix = Mat::zeros(3,3,CV_32FC1);
Matx33f rotationMatrix ;// = Mat::zeros(3,3,CV_32FC1);


rotationMatrix.at<float>(0,0) = c3*c2;
rotationMatrix.at<float>(0,1) = c3*s2*s1-s3*c1;
rotationMatrix.at<float>(0,2) = c3*s2*c1+s3*s1;
rotationMatrix(0,0) = c3*c2;
rotationMatrix(0,1) = c3*s2*s1-s3*c1;
rotationMatrix(0,2) = c3*s2*c1+s3*s1;

rotationMatrix.at<float>(1,0) = s3*c2;
rotationMatrix.at<float>(1,1) = s3*s2*s1+c3*c1;
rotationMatrix.at<float>(1,2) = s3*s2*c1-c3*s1;
rotationMatrix(1,0) = s3*c2;
rotationMatrix(1,1) = s3*s2*s1+c3*c1;
rotationMatrix(1,2) = s3*s2*c1-c3*s1;

rotationMatrix.at<float>(2,0) = -s2;
rotationMatrix.at<float>(2,1) = c2*s1;
rotationMatrix.at<float>(2,2) = c2*c1;
rotationMatrix(2,0) = -s2;
rotationMatrix(2,1) = c2*s1;
rotationMatrix(2,2) = c2*c1;

/*
worldVector.x = c3*c2*acc.x + (c3*s2*s1-s3*c1)*acc.y+(c3*s2*c1+s3*s1)*acc.z;
Expand Down
Loading

0 comments on commit 2c35a6b

Please sign in to comment.