├── Dataset
    └── dataset
├── example
    ├── result1.png
    └── result2.png
├── CMakeLists.txt
├── include
    ├── matcher.hpp
    ├── registration.hpp
    ├── neighborhood.hpp
    ├── toolfile.hpp
    └── pointCloudMapping.hpp
├── LICENSE
├── README.md
└── src
    ├── main.cpp
    ├── neighborhood.cpp
    ├── matcher.cpp
    ├── registration.cpp
    ├── toolfile.cpp
    └── pointCloudMapping.cpp


/Dataset/dataset:
--------------------------------------------------------------------------------
1 | download the dataset and save the files here
2 | 


--------------------------------------------------------------------------------
/example/result1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FreeformRobotics/semantic-histogram-based-global-localization/HEAD/example/result1.png


--------------------------------------------------------------------------------
/example/result2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FreeformRobotics/semantic-histogram-based-global-localization/HEAD/example/result2.png


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | 
 3 | set(CMAKE_BUILD_TYPE Release)
 4 | set(DCMAKE_BUILD_TYPE Release)
 5 | 
 6 | project(mapAlignment)
 7 | 
 8 | find_package(PCL REQUIRED)
 9 | include_directories(${PCL_INCLUDE_DIRS})
10 | add_definitions(${PCL_DEFINITIONS})
11 | 
12 | find_package(OpenCV REQUIRED)
13 | include_directories(${OpenCV_INCLUDE_DIRS})
14 | 
15 | find_package(Eigen3 REQUIRED)
16 | include_directories(${EIGEN3_INCLUDE_DIRS})
17 | 
18 | find_package(Pangolin REQUIRED)
19 | include_directories(${Pangolin_INCLUDE_DIRS})
20 | 
21 | include_directories(${PROJECT_SOURCE_DIR}/include)
22 | 
23 | FILE(GLOB SRC_FILES "src/*.cpp")
24 | 
25 | 
26 | add_executable(mapAlignment ${SRC_FILES})
27 | 
28 | target_link_libraries(mapAlignment ${PCL_LIBRARIES} ${Pangolin_LIBRARIES} ${OpenCV_LIBS} ${EIGEN3_LIBRARIES})
29 | 
30 | 


--------------------------------------------------------------------------------
/include/matcher.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef matcher_hpp
 2 | #define matcher_hpp
 3 | 
 4 | #include "opencv2/opencv.hpp"
 5 | #include "opencv2/core.hpp"
 6 | #include <iostream>  
 7 | #include <stdio.h>
 8 | 
 9 | #include <eigen3/Eigen/Dense>
10 | #include <eigen3/Eigen/Sparse>
11 | #include <vector>
12 | #include "neighborhood.hpp"
13 | 
14 | 
15 | using namespace cv;
16 | using namespace std;
17 | using namespace Eigen;
18 | 
19 | class matcher
20 | {
21 |     public:
22 |         matcher(Descriptor Ds1, Descriptor Ds2);
23 |         matcher(Descriptor Ds1, Descriptor Ds2, int type);
24 |         ~matcher();
25 |         MatrixXf scoreMatrix;
26 |         MatrixXi getMatcherID();
27 |         MatrixXi getGoodMatcher();
28 | 
29 |     private:
30 |         int size1;
31 |         int size2;
32 |         VectorXf noNei;
33 | };
34 | 
35 | #endif /* pointCloudMapping */
36 | 
37 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2022 Freeform Robotics
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # semantic-histogram-based-global-localization
 2 | The global localization system based on semantic information
 3 | 
 4 | paper link: https://arxiv.org/abs/2010.09297 or https://ieeexplore.ieee.org/document/9353207/
 5 | 
 6 | Results
 7 | -
 8 | ![](https://github.com/gxytcrc/Semantic-Graph-based--global-Localization/blob/main/example/result1.png)
 9 | ![](https://github.com/gxytcrc/Semantic-Graph-based--global-Localization/blob/main/example/result2.png)
10 | 
11 | # 1. Prerequisites #
12 | * Ubuntu
13 | * CMake
14 | * Eigen
15 | * Pangolin
16 | * OpenCV
17 | * PCL
18 | 
19 | # 2. Running #
20 | Clone the repository and catkin_make:
21 | ```
22 |     git clone https://https://github.com/FreeformRobotics/Semantic-Graph-based--global-Localization.git
23 |     mkdir build
24 |     cd build
25 |     cmake ..
26 |     catkin_make
27 | ```
28 | Download the dataset that is created from Airsim, and save them into the Datset . Download link: https://drive.google.com/file/d/1PIuRyah6lKDTe_YJ6HSVRX4l7MQLwKq5/view?usp=sharing. 
29 | 
30 | Launch it as follows:
31 | ```
32 | ./mapAlignment robot1-foldername startFrameNumber endFrameNumber robot2-foldername startFrameNumber endFrameNumber
33 | ```
34 | 


--------------------------------------------------------------------------------
/include/registration.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef registration_hpp
 2 | #define registration_hpp
 3 | 
 4 | 
 5 | #include "opencv2/opencv.hpp"
 6 | #include "opencv2/core.hpp"
 7 | #include <iostream>  
 8 | #include <stdio.h>
 9 | #include <eigen3/Eigen/Dense>
10 | #include <eigen3/Eigen/Geometry>
11 | #include <eigen3/Eigen/Core>
12 | #include <Eigen/SVD> 
13 | #include <vector>
14 | #include <ctime>
15 | 
16 | using namespace cv;
17 | using namespace std;
18 | using namespace Eigen;
19 | 
20 | class registration
21 | {
22 |     public:
23 |         registration(vector<vector<float> > centerpoint1, vector<vector<float> > centerpoint2, MatrixXi matcherID);
24 |         ~registration();
25 |         void matcherRANSAC(float Td);
26 |         void Alignment();
27 |         MatrixXi inlierID;
28 |         MatrixXf Rotation, Translation;
29 | 
30 |     private:
31 |         MatrixXf sourcePoints;
32 |         MatrixXf targetPoints;
33 |         MatrixXf sourceInliers;
34 |         MatrixXf targetInliers;
35 |         MatrixXi totalID;
36 |         VectorXi inlierTotalID;
37 |         VectorXi labelVector;
38 | 
39 | };
40 | 
41 | void getTransform(MatrixXf source, MatrixXf target);
42 | #endif /* pointCloudMapping */
43 | 


--------------------------------------------------------------------------------
/include/neighborhood.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef neighborhood_hpp
 2 | #define neighborhood_hpp
 3 | 
 4 | #include "opencv2/opencv.hpp"
 5 | #include "opencv2/core.hpp"
 6 | #include <iostream>  
 7 | #include <stdio.h>
 8 | 
 9 | #include <eigen3/Eigen/Dense>
10 | #include <eigen3/Eigen/Sparse>
11 | #include <pangolin/pangolin.h>
12 | #include <vector>
13 | 
14 | #include <cstdlib>
15 | #include <ctime>
16 | 
17 | 
18 | using namespace cv;
19 | using namespace std;
20 | using namespace Eigen;
21 | 
22 | class Neighborhood
23 | {
24 |     public:
25 |         Neighborhood(vector<vector<float> > Cpoint);
26 |         ~Neighborhood();
27 |         MatrixXf getNeighbor();
28 |         VectorXf label;
29 |     private:
30 |         vector<vector<float> > centerpoint;
31 |         
32 | };
33 | 
34 | class Descriptor
35 | {
36 |     public:
37 |         Descriptor(Neighborhood Nei, int stepNumber, int stepLengh);
38 |         Descriptor(Neighborhood Nei);
39 |         Descriptor(Neighborhood Nei, int Histogram);
40 |         ~Descriptor();
41 |         MatrixXf getDescriptor(int DesID);
42 |         void draw(int DesID, Mat lab);
43 |         int size();
44 |         int SL;
45 |         int SN;
46 |         VectorXf noNeighbor;
47 |         VectorXf labelVector;
48 | 
49 | 
50 |     private:
51 |         MatrixXf neighbor;
52 |         vector<MatrixXf> DescriptorVector;
53 | };
54 | 
55 | 
56 | 
57 | #endif /* pointCloudMapping */
58 | 


--------------------------------------------------------------------------------
/include/toolfile.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef  vo_hpp
 2 | #define vo_hpp
 3 | 
 4 | #include "opencv2/video/tracking.hpp"
 5 | #include "opencv2/imgproc/imgproc.hpp"
 6 | #include "opencv2/highgui/highgui.hpp"
 7 | #include "opencv2/features2d/features2d.hpp"
 8 | #include "opencv2/calib3d/calib3d.hpp"
 9 | #include <eigen3/Eigen/Dense>
10 | #include <eigen3/Eigen/SVD>
11 | 
12 | 
13 | 
14 | #include <iostream>
15 | #include <ctype.h>
16 | #include <algorithm>
17 | #include <iterator>
18 | #include <vector>
19 | #include <ctime>
20 | #include <sstream>
21 | #include <fstream>
22 | #include <string>
23 | #include <iostream>
24 | 
25 | #include "pointCloudMapping.hpp"
26 | 
27 | 
28 | using namespace cv;
29 | using namespace std;
30 | using namespace Eigen;
31 | 
32 | void insertPose(string&dir, Mat &pose, int number);
33 | void insertSYNTIAPose(string&dir, Mat&pose, int number);
34 | 
35 | cv::Mat Quaternion2Matrix (cv::Mat q);
36 | 
37 | void gatherPointCloudData(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, vector<vector<float> >& centerpoint, Mat pose, Mat Label, vector<uchar> label_gray, vector<float> camera, float scale, string dir, int fileNumber, int startpoint);
38 | void gatherDenseMap(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, vector<vector<float> >& centerpoint, Mat pose, Mat Label, vector<float> camera, float scale, string dir, int fileNumber, int type);
39 | void calculateGroundTruth(Mat pose1, Mat pose2, MatrixXf Rotation, MatrixXf translation, Matrix4f& T);
40 | void matchingEvaluation(vector<vector<float> > Cpoint1, vector<vector<float> > Cpoint2, MatrixXi matcherID, MatrixXi InlierID, float& Pvalue, float& Rvalue, float& Pcount1);
41 | void outputCenterpoint(vector<vector<float> >& centerpoint, string txtname);
42 | void inputCenterpoint(vector<vector<float> >& centerpoint, int number, string txtname);
43 | 
44 | 
45 | #endif /* pointCloudMapping */
46 | 


--------------------------------------------------------------------------------
/include/pointCloudMapping.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef pointCloudMapping_hpp
 2 | #define pointCloudMapping_hpp
 3 | 
 4 | #include "opencv2/opencv.hpp"
 5 | #include "opencv2/core.hpp"
 6 | #include "opencv2/flann.hpp"
 7 | #include <opencv2/calib3d.hpp>
 8 | #include <opencv2/opencv.hpp>  
 9 | #include <pcl/io/ply_io.h>
10 | #include <pcl/point_types.h>
11 | #include <pcl/visualization/pcl_visualizer.h>
12 | #include <pcl/visualization/cloud_viewer.h>
13 | #include <pcl/filters/radius_outlier_removal.h>
14 | #include <pcl/filters/conditional_removal.h>
15 | #include <iostream>  
16 | #include <stdio.h>
17 | #include <boost/make_shared.hpp>
18 | #include <pcl/common/transforms.h>
19 | #include <pcl/filters/voxel_grid.h>
20 | #include <eigen3/Eigen/Dense>
21 | #include <pangolin/pangolin.h>
22 | #include<vector>
23 | #include "toolfile.hpp"
24 | 
25 | 
26 | 
27 | using namespace cv;
28 | using namespace std;
29 | using namespace pcl;
30 | using namespace Eigen;
31 | 
32 | struct pointCloudMapping
33 | {
34 | public:
35 |     
36 |     void insertValue(pcl::PointCloud<pcl::PointXYZRGB>::Ptr&insertCloud, Mat&depthmap, Mat&image1, Matrix4f&TransformsValue, vector<float>&camera, float scale, vector<vector<float> > Centerpoint, Mat Labels);
37 |     void initialize3Dmap(int type);
38 |     void pointCloudFilter();
39 |     void pointExtraction(vector<uchar> label);
40 |     void pointExtraction(vector<uchar> label, Mat labelimag);
41 |     void pointVisuallize(pcl::PointCloud<pcl::PointXYZRGB>::Ptr&insertCloud1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr&insertCloud2, MatrixXi matcherID, MatrixXf R, MatrixXf T);
42 |     vector<vector<float> > Cpoint;
43 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputPointCloud();
44 |     
45 | private:
46 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr Cloud;
47 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr GlobalCloud;
48 |     Matrix4f T = Matrix4f::Identity();
49 |     Mat depth;
50 |     Mat image;
51 |     Mat LabelValue;
52 |     float fx; 
53 |     float fy; 
54 |     float cx; 
55 |     float cy;
56 |     float baseline;
57 |     float s;         
58 | };
59 | 
60 | #endif /* pointCloudMapping */
61 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
  1 | #include "pointCloudMapping.hpp"
  2 | #include "neighborhood.hpp"
  3 | #include "toolfile.hpp"
  4 | #include <string.h>
  5 | #include <boost/thread/thread.hpp>
  6 | #include "matcher.hpp"
  7 | #include "registration.hpp"
  8 | 
  9 | 
 10 | using namespace cv;
 11 | using namespace std;
 12 | using namespace Eigen;
 13 | 
 14 | 
 15 | int main(int argc, const char * argv[])
 16 | {
 17 |     if(argc!=7)
 18 |     {
 19 |         cout << "usage: ./SLAM_Project address frameCount" << endl;
 20 |         return 1;
 21 |     }
 22 | 
 23 |     char file_name1[1024];
 24 |     char fullpath1[1024];
 25 |     char file_name2[1024];
 26 |     char fullpath2[1024];
 27 | 
 28 |     sprintf(file_name1, "%s/", argv[1]);
 29 |     sprintf(fullpath1,"../Dataset/%s",file_name1);
 30 |     int startPoint1 = atoi(argv[2]);
 31 |     int fileNumber1 = atoi(argv[3]);
 32 |     cout<<"file number is: "<<fileNumber1<<endl;
 33 |     sprintf(file_name2, "%s/", argv[4]);
 34 |     sprintf(fullpath2,"../Dataset/%s",file_name2);
 35 |     int startPoint2 = atoi(argv[5]);
 36 |     int fileNumber2 = atoi(argv[6]);
 37 | 
 38 |     string dir1 = fullpath1;
 39 |     string dir2 = fullpath2;
 40 |     //generate the camera parameter
 41 |     vector<float> camera(4);
 42 |     float scale = 1;
 43 |     camera[0] = 512; //fx
 44 |     camera[1] = 512; //fy
 45 |     camera[2] = 512; //cx
 46 |     camera[3] = 288;//cy
 47 | 
 48 |     //iniltialize the R and T
 49 |     MatrixXf R, T;
 50 |     R.setIdentity();
 51 |     T.setZero();
 52 | 
 53 |     //create the value for visual odometry
 54 |     Mat frame_pose = cv::Mat::eye(4, 4, CV_64F);
 55 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
 56 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZRGB>);
 57 | 
 58 |     Mat pose1 = cv::Mat::zeros(1000, 7, CV_64FC1);
 59 |     Mat pose2 = cv::Mat::zeros(1000, 7, CV_64FC1);
 60 | 
 61 |     //generate the correspondent rgb value of all labels in the segmentation image
 62 |     Mat Label = (cv::Mat_<int>(11, 3)<< 175, 6, 140,
 63 |                                     65, 54, 217,
 64 |                                     156, 198, 23,
 65 |                                     184, 145, 182,
 66 |                                     211, 80, 208,
 67 |                                     232, 250, 80,
 68 |                                     234, 20, 250,
 69 |                                     99, 242, 104,
 70 |                                     142, 1, 246,
 71 |                                     81, 13, 36,
 72 |                                     112, 105, 191);
 73 | 
 74 |     //Obtain the label number
 75 |     vector<uchar> label_gray(11);
 76 |     label_gray[0] = 72; label_gray[1] = 76; label_gray[2] = 165; label_gray[3] = 161; label_gray[4] = 134;
 77 |     label_gray[5] = 225; label_gray[6] = 110; label_gray[7] = 184; label_gray[8] = 71; label_gray[9] = 36;
 78 |     label_gray[10] = 117;
 79 | 
 80 |     //insert the odeometry value
 81 |     insertPose(dir1,pose1, 1000);
 82 |     insertPose(dir2,pose2, 1000);
 83 |     
 84 |     //initiallize the intial value of the keypoint;
 85 |     vector<vector<float> > centerpoint2;
 86 |     vector<vector<float> > centerpoint1;
 87 |     
 88 |     gatherPointCloudData(cloud1, centerpoint1, pose1, Label, label_gray, camera, scale, dir1, fileNumber1, startPoint1);
 89 |     gatherPointCloudData(cloud2, centerpoint2, pose2, Label, label_gray, camera, scale, dir2, fileNumber2, startPoint2);
 90 |     
 91 |     //add the edge between the neighborhood
 92 |     Neighborhood Nei1(centerpoint1);
 93 | 
 94 |     //MatrixXf neighbor;
 95 |     Neighborhood Nei2(centerpoint2);
 96 |     //neighbor = Nei2.getNeighbor();
 97 |     //cout<<neighbor<<endl;
 98 | 
 99 |     //obtain the descriptor
100 |     MatrixXf descriptor1;
101 |     Descriptor Des1(Nei1, 1);
102 |     //descriptor1 = Des1.getDescriptor(4);
103 |     //cout<<descriptor1<<endl;
104 | 
105 |     MatrixXf descriptor2;
106 |     Descriptor Des2(Nei2, 1);
107 |     // descriptor2 = Des2.getDescriptor(23);
108 |     // cout<<descriptor2<<endl;
109 |     
110 |     //matching
111 |     MatrixXi matcherID;
112 |     matcher matches(Des1, Des2, 2);
113 |     matcherID = matches.getGoodMatcher();
114 |     //cout<<matcherID<<endl;
115 | 
116 |     //begain registration
117 |     //insert the value
118 |     registration registration(centerpoint1, centerpoint2, matcherID);
119 | 
120 |     //reject the outliers with ICP-RANSAC method
121 |     registration.matcherRANSAC(10);
122 |     MatrixXi inlierID;
123 |     inlierID = registration.inlierID;
124 | 
125 |     //final pose estiamation
126 |     registration.Alignment();
127 |     
128 |     R = registration.Rotation;
129 |     T = registration.Translation;
130 |     
131 |     // float finalDistance = sqrt(pow(T(0, 0), 2) + pow(T(1, 0),2) + pow(T(2, 0),2));
132 |     
133 |     // cout<<"final distance: "<<finalDistance<<endl;
134 | 
135 | 
136 |     // //plot the semantic point and matching with PCL library  
137 |     // pointCloudMapping pointCloudMapping;
138 |     // pointCloudMapping.pointVisuallize(cloud1, cloud2, inlierID, R, T);
139 |     
140 | }
141 | 


--------------------------------------------------------------------------------
/src/neighborhood.cpp:
--------------------------------------------------------------------------------
  1 | #include "neighborhood.hpp"
  2 | 
  3 | Neighborhood::Neighborhood(vector<vector<float> > Cpoint){
  4 |     cout<<"Neighborhood construct"<<endl;
  5 |     int size = Cpoint.size();
  6 |     label.resize(size);
  7 |     for(int i =0; i<size; i++){
  8 |         vector<float> cp(4);
  9 |         cp[0] = Cpoint[i][0]; cp[1] = Cpoint[i][1]; cp[2] = Cpoint[i][2]; cp[3] = Cpoint[i][3]; 
 10 |         label(i) = cp[3];
 11 |         centerpoint.push_back(cp);
 12 |     }
 13 | }
 14 | 
 15 | Neighborhood::~Neighborhood(){
 16 |     cout<<"The Neighborhood object destroyed"<<endl;
 17 | }
 18 | 
 19 | MatrixXf Neighborhood::getNeighbor(){
 20 | 
 21 |     MatrixXf neighbor(centerpoint.size(), centerpoint.size());
 22 |     neighbor.setZero();
 23 |     for(int n = 0; n < centerpoint.size(); n++){
 24 |         float x0 = centerpoint[n][0];
 25 |         float y0 = centerpoint[n][1];
 26 |         float z0 = centerpoint[n][2];
 27 |         for(int m = n+1; m<centerpoint.size(); m++){
 28 |             float x1 = centerpoint[m][0];
 29 |             float y1 = centerpoint[m][1];
 30 |             float z1 = centerpoint[m][2];
 31 |             float distance = sqrt(pow(x0-x1,2)+ pow(y0-y1,2) + pow(z0-z1,2));
 32 |             // cout<<"label0: "<<centerpoint[n][3]<<endl;
 33 |             // cout<<"label1: "<<centerpoint[m][3]<<endl;
 34 |             // cout<<"distance: "<<distance<<endl;
 35 | 
 36 |             if(distance<20){
 37 |                 neighbor(m, n) = 1;
 38 |                 neighbor(n, m) = 1;
 39 |             }
 40 |         }
 41 |     }
 42 |     return neighbor;
 43 | 
 44 | }
 45 | 
 46 | //-----------------------------------------------------------------------------------------------
 47 | // Random walk descriptor
 48 | Descriptor::Descriptor(Neighborhood Nei, int stepNumber, int stepLengh){
 49 |     clock_t startTime,endTime;
 50 |     startTime = clock();
 51 |     cout<<"Descriptor construct"<<endl;
 52 |     //obtain the neibore relations;
 53 |     neighbor = Nei.getNeighbor();
 54 |     labelVector = Nei.label;
 55 |     cout<<"vector size: "<<labelVector.size()<<endl;
 56 |     //cout<<labelVector<<endl;
 57 |     //inite the no neighbor vector;
 58 |     noNeighbor.resize(labelVector.size());
 59 |     noNeighbor.setZero();
 60 |     SN = stepNumber;
 61 |     SL = stepLengh;
 62 |     endTime = clock();
 63 |     int timedifference = (int)((endTime - startTime)*10000);
 64 |     int size = neighbor.rows();
 65 |     cout<<"time1: "<<time(0)<<endl;
 66 |     srand((int)time(0)+timedifference);
 67 |     cout<<"test1"<<endl;
 68 |     for(int row = 0; row<size; row++){
 69 | 
 70 |         MatrixXf SingleDescriptor(SN, SL);
 71 |         float label;
 72 |         float PointID;
 73 |         for(int stepNum = 0; stepNum<SN; stepNum++){
 74 |             for(int stepDepth = 0; stepDepth<SL; stepDepth++){
 75 |                 if(stepDepth == 0){
 76 |                     PointID = row;
 77 |                 }
 78 |                 label = labelVector(PointID);
 79 |                 SingleDescriptor(stepNum, stepDepth) = label;
 80 |                 
 81 |                 //find the neighbor point
 82 |                 int count = 0;
 83 |                 vector<int> neighborID;
 84 |                 for(int IDnum = 0; IDnum<size; IDnum++){
 85 |                     if(neighbor(PointID, IDnum) == 1){
 86 |                         neighborID.push_back(IDnum);
 87 |                         count = count+1;
 88 |                     }
 89 |                 }
 90 |                 if(count == 0){
 91 |                     noNeighbor(PointID) = 1;
 92 |                     continue;
 93 |                 }
 94 |                 int pathID = rand()%count;
 95 |                 PointID = neighborID[pathID];
 96 |             }
 97 |         }
 98 | 
 99 |         DescriptorVector.push_back(SingleDescriptor);
100 |     }
101 | 
102 | 
103 | }
104 | //FAST Descriptor
105 | Descriptor::Descriptor(Neighborhood Nei){
106 |     cout<<"Descriptor construct"<<endl;
107 |     neighbor = Nei.getNeighbor();
108 |     labelVector = Nei.label;
109 |     //inite the no neighbor vector;
110 |     noNeighbor.resize(labelVector.size());
111 |     noNeighbor.setZero();
112 |     int size = neighbor.rows();
113 |     int labelSize = 11;
114 |     for(int Num =0; Num<size; Num++){
115 |         MatrixXf SingleDescriptor(labelSize, 1);
116 |         SingleDescriptor.setZero();
117 |         //colmum 2
118 |         //find the neibor point
119 |         int neiCount = 0;
120 |         vector<int> neighborID;
121 |         for(int NeiNum = 0; NeiNum<size; NeiNum++){
122 |             if(neighbor(Num, NeiNum) == 1){
123 |                 int locallabel = labelVector(NeiNum);
124 |                 SingleDescriptor(locallabel, 0) = SingleDescriptor(locallabel, 0) + 1;
125 |                 neighborID.push_back(NeiNum);
126 |                 neiCount = neiCount+1;
127 |             }
128 |         }
129 |         if(neiCount ==0){
130 |             noNeighbor(Num) = 1;
131 |             DescriptorVector.push_back(SingleDescriptor);
132 |             continue;
133 |         }
134 |         DescriptorVector.push_back(SingleDescriptor);
135 |     }
136 | }
137 | 
138 | Descriptor::Descriptor(Neighborhood Nei, int Histogram){
139 |     cout<<"Descriptor construct"<<endl;
140 |     neighbor = Nei.getNeighbor();
141 |     labelVector = Nei.label;
142 |     //inite the no neighbor vector;
143 |     noNeighbor.resize(labelVector.size());
144 |     noNeighbor.setZero();
145 |     int size = neighbor.rows();
146 |     int labelSize = 11;
147 |     for(int Num =0; Num<size; Num++){
148 |         MatrixXf SingleDescriptor(1331, 1);
149 |         SingleDescriptor.setZero();
150 |         int VectorNum = 0;
151 |         //colmum 2
152 |         //find the neibor point
153 |         int neiCount = 0;
154 |         vector<int> neighborID;
155 |         for(int NeiNum = 0; NeiNum<size; NeiNum++){
156 |             if(neighbor(Num, NeiNum) == 1){
157 |                 neighborID.push_back(NeiNum);
158 |                 neiCount = neiCount+1;
159 |             }
160 |         }
161 |         if(neiCount ==0){
162 |             noNeighbor(Num) = 1;
163 |             DescriptorVector.push_back(SingleDescriptor);
164 |             continue;
165 |         }
166 |         //colmum 3
167 |         int pointlable = labelVector(Num);
168 |         for(int NeiID =0; NeiID<neiCount; NeiID++){
169 | 
170 |             int NeiNum = neighborID[NeiID];
171 |             int previoudlable = labelVector(NeiNum);
172 | 
173 |             for(int NeiNeiNum = 0; NeiNeiNum<size; NeiNeiNum++){
174 |                 if(neighbor(NeiNum, NeiNeiNum) == 1){
175 |                     int locallabel = labelVector(NeiNeiNum);
176 |                     int VectorNum = pointlable * 121 + previoudlable*11 + locallabel;
177 |                     SingleDescriptor(VectorNum, 0) = SingleDescriptor(VectorNum, 0) + 1;
178 |                 }
179 |             }
180 |         }
181 |         DescriptorVector.push_back(SingleDescriptor);
182 |     }
183 | }
184 | 
185 | Descriptor::~Descriptor(){
186 |     cout<<"The Descriptor object destroyed"<<endl;
187 | }
188 | 
189 | MatrixXf Descriptor::getDescriptor(int DesID){
190 |     MatrixXf SingleDescriptor;
191 |     SingleDescriptor = DescriptorVector[0];
192 |     int rows =  SingleDescriptor.rows();
193 |     int cols = SingleDescriptor.cols();
194 |     
195 |     int des_size = DescriptorVector.size();
196 |     //cout<<"des size: "<<des_size<<endl;
197 |     for(int i = 0; i<des_size; i++){
198 |         if(i == DesID){
199 |             return DescriptorVector[i];
200 |         }
201 |     }
202 |     cout<<"error, no this ID's Descriptor"<<endl;
203 |     MatrixXf errorMat(rows,cols);
204 |     errorMat.setZero();
205 |     return errorMat;
206 | }
207 | 
208 | //can only draw the RandomWalk descriptor
209 | void Descriptor::draw(int DesID, Mat lab){
210 |     int row = SN*100;
211 |     int col = SL*100;
212 |     Mat DescriptorPlot = cv::Mat::zeros(row, col, CV_8UC3);
213 |     MatrixXf singleDescriptor = DescriptorVector[DesID];
214 |     for(int i =0; i<SN; i++){
215 |         for(int j=0; j<SL; j++){
216 |             int startPointi = i*100;
217 |             int endPointi = i*100+99;
218 |             int startPointj = j*100;
219 |             int endPointj = j*100+99;
220 |             int label = (int)singleDescriptor(i, j);
221 |             int R = lab.at<int>(label, 0);
222 |             int G = lab.at<int>(label, 1);
223 |             int B = lab.at<int>(label, 2);
224 |             for(int pointi = startPointi; pointi<endPointi; pointi++){
225 |                 for(int pointj = startPointj; pointj<endPointj; pointj++){
226 |                     DescriptorPlot.at<Vec3b>(pointi, pointj)[0] = B;
227 |                     DescriptorPlot.at<Vec3b>(pointi, pointj)[1] = G;
228 |                     DescriptorPlot.at<Vec3b>(pointi, pointj)[2] = R;
229 |                 }
230 |             }
231 |         }
232 |     }
233 |     imshow("descriptor", DescriptorPlot);
234 |     waitKey(1000000);
235 | }
236 | 
237 | int Descriptor::size(){
238 |     int des_size = DescriptorVector.size();
239 |     return des_size;
240 | }
241 | 


--------------------------------------------------------------------------------
/src/matcher.cpp:
--------------------------------------------------------------------------------
  1 | #include "matcher.hpp"
  2 | #include "neighborhood.hpp"
  3 | 
  4 | //Random walk decriptor matching
  5 | matcher::matcher(Descriptor Ds1, Descriptor Ds2){
  6 |     cout<<"matcher constructed"<<endl;
  7 |     clock_t startTime,endTime;
  8 |     noNei = Ds1.noNeighbor;
  9 |     //the sum number of the descriptors
 10 |     size1 = Ds1.size();
 11 |     size2 = Ds2.size();
 12 |     //obtain the decriptors' matrix size
 13 |     int matrixRow = Ds1.SN;
 14 |     int matrixCol = Ds1.SL;
 15 |     int sumNumber = matrixRow;
 16 |     cout<<"Des matrixRow: "<<matrixRow<<endl;
 17 |     scoreMatrix.resize(size1, size2);
 18 |     scoreMatrix.setZero();
 19 |     for(int i = 0; i<size1; i++){
 20 |         for(int j = 0; j<size2; j++){
 21 |             MatrixXf descriptor1 = Ds1.getDescriptor(i);
 22 |             MatrixXf descriptor2 = Ds2.getDescriptor(j);
 23 |             if(descriptor1(0, 0) != descriptor2(0, 0) || descriptor1(0, 0) == 100 ){
 24 |                 scoreMatrix(i, j) = 0;
 25 |                 continue;
 26 |             }
 27 |             int scoreCount1 = 0;
 28 |             int scoreCount2 = 0;
 29 |             startTime = clock();
 30 |             for(int row1 = 0; row1<matrixRow; row1++){
 31 |                 
 32 |                 //descriptor1 matches descriptor2
 33 |                 //obatin vector from the descriptor matrix1
 34 |                 VectorXf vector1;
 35 |                 vector1.resize(matrixCol);
 36 |                 vector1 = descriptor1.row(row1);
 37 |                 //cout<<"vector: "<<vector1<<endl;
 38 |                 int maxCount = 0;
 39 |                 for(int row2 = 0; row2<matrixRow; row2++){
 40 |                     //obatin vector from the descriptor matrix2
 41 |                     VectorXf vector2;
 42 |                     vector2.resize(matrixCol);
 43 |                     vector2 = descriptor2.row(row2);
 44 |                     //init a count number;
 45 |                     if(vector1 == vector2){
 46 |                        maxCount = 1;
 47 |                        break;
 48 |                     }
 49 |                 }
 50 |                 scoreCount1 = scoreCount1 + maxCount;
 51 |             }
 52 | 
 53 |             for(int row1 = 0; row1<matrixRow; row1++){
 54 | 
 55 |                 //descriptor2 matches descriptor1
 56 |                 //obatin vector from the descriptor matrix2
 57 |                 VectorXf _vector1;
 58 |                 _vector1.resize(matrixCol);
 59 |                 _vector1 = descriptor2.row(row1);
 60 |                 int _maxCount = 0;
 61 |                 for(int row2 = 0; row2<matrixRow; row2++){
 62 |                     //obatin vector from the descriptor matrix1
 63 |                     VectorXf _vector2;
 64 |                     _vector2.resize(matrixCol);
 65 |                     _vector2 = descriptor1.row(row2);
 66 |                     //init a count number;
 67 |                     if(_vector1 == _vector2){
 68 |                         _maxCount = 1;
 69 |                         break;
 70 |                     }
 71 |                 }
 72 |                 scoreCount2 = scoreCount2 + _maxCount;
 73 | 
 74 |             }
 75 | 
 76 |             //use the smaller score as the final score value
 77 |             float finalScore1 = (float)scoreCount1;
 78 |             float finalScore2 = (float)scoreCount2;
 79 | 
 80 |             if(scoreCount1<scoreCount2){
 81 |                 scoreMatrix(i, j) = finalScore1/sumNumber;
 82 |             }
 83 |             else{
 84 |                 scoreMatrix(i, j) = finalScore2/sumNumber;
 85 |             }
 86 |             endTime = clock();
 87 |             double timedifference = (double)(endTime - startTime)/CLOCKS_PER_SEC;
 88 |         }
 89 |         
 90 |     }
 91 |     //cout<<scoreMatrix<<endl;
 92 | }
 93 | 
 94 | //FAST Descriptor matching
 95 | matcher::matcher(Descriptor Ds1, Descriptor Ds2, int type){
 96 |     if(type == 1){
 97 | 
 98 |         cout<<"matcher constructed"<<endl;
 99 |         noNei = Ds1.noNeighbor;
100 |         //the sum number of the descriptors
101 |         size1 = Ds1.size();
102 |         size2 = Ds2.size();
103 |         MatrixXf descriptor0 = Ds1.getDescriptor(0);
104 |         int matrixRow = descriptor0.rows();
105 |         int matrixCol = descriptor0.cols();
106 |         scoreMatrix.resize(size1, size2);
107 |         scoreMatrix.setZero();
108 | 
109 |         for(int i = 0; i<size1; i++){
110 |             float averageDes1 = 0;
111 |             MatrixXf descriptor1 = Ds1.getDescriptor(i);
112 |             for(int row = 0; row<matrixRow; row++){
113 |                 for(int col = 0; col<matrixCol; col++){
114 |                     averageDes1 = averageDes1 + descriptor1(row, col);
115 |                 }
116 |             }
117 |             averageDes1 = averageDes1/(matrixRow*matrixCol);
118 |             //cout<<averageDes1<<endl;
119 |             for(int j = 0; j<size2; j++){
120 |                 float score = 0;
121 |                 float averageDes2 = 0;
122 |                 MatrixXf descriptor2 = Ds2.getDescriptor(j);
123 |                 if(descriptor1(0,0) != descriptor2(0, 0)){
124 |                     scoreMatrix(i, j) = 0;
125 |                     continue;
126 |                 }
127 |                 for(int row = 0; row<matrixRow; row++){
128 |                     for(int col = 0; col<matrixCol; col++){
129 |                         averageDes2 = averageDes2 + descriptor2(row, col);
130 |                     }
131 |                 }
132 |                 averageDes2 = averageDes2/(matrixRow*matrixCol);
133 |                 //caculate the corrlation
134 |                 float SumTop = 0; 
135 |                 for(int row = 0; row<matrixRow; row++){
136 |                     for(int col = 0; col<matrixCol; col++){
137 |                         SumTop = SumTop + ((descriptor1(row, col) - averageDes1)*(descriptor2(row, col) - averageDes2));
138 |                     }
139 |                 }
140 |                 float SumBottomLeft = 0; 
141 |                 float SumBottomRight = 0; 
142 |                 for(int row = 0; row<matrixRow; row++){
143 |                     for(int col = 0; col<matrixCol; col++){
144 |                         SumBottomLeft = SumBottomLeft + (pow((descriptor1(row, col) - averageDes1),2));
145 |                         SumBottomRight = SumBottomRight + (pow((descriptor2(row, col) - averageDes2),2));
146 |                     }
147 |                 }
148 |                 float SumBottom = sqrt(SumBottomLeft*SumBottomRight);
149 |                 score = SumTop/SumBottom;
150 |                 scoreMatrix(i, j) = abs(score);
151 |             }
152 |         }
153 | 
154 |         //cout<<scoreMatrix<<endl;
155 | 
156 |     }
157 |     else{
158 |         cout<<"matcher constructed"<<endl;
159 |         noNei = Ds1.noNeighbor;
160 |         //the sum number of the descriptors
161 |         size1 = Ds1.size();
162 |         size2 = Ds2.size();
163 | 
164 |         scoreMatrix.resize(size1, size2);
165 |         scoreMatrix.setZero();
166 | 
167 |         MatrixXf descriptor0 = Ds1.getDescriptor(0);
168 |         int Num = descriptor0.rows();
169 | 
170 |         VectorXf lablevector1 = Ds1.labelVector;
171 |         VectorXf lablevector2 = Ds2.labelVector;
172 |         
173 |         for(int i = 0; i<size1; i++ ){
174 |             MatrixXf descriptor1 = Ds1.getDescriptor(i);
175 |             int lab1 = lablevector1(i);
176 |             if(noNei(i) == 1){
177 |                 continue;
178 |             }
179 |             for(int j = 0; j<size2; j++){
180 |                 float score = 0;
181 |                 MatrixXf descriptor2 = Ds2.getDescriptor(j);
182 |                 int lab2 = lablevector2(j);
183 |                 if(lab1 != lab2){
184 |                     scoreMatrix(i, j) = 0;
185 |                     continue;
186 |                 }
187 |                 float SumTop = 0;
188 |                 for(int row = 0; row<Num; row++){
189 |                     SumTop = SumTop + (descriptor1(row, 0)*descriptor2(row, 0));
190 |                 }
191 |                 float SumBottomLeft = 0; 
192 |                 float SumBottomRight = 0; 
193 |                 for(int row = 0; row<Num; row++){
194 |                     SumBottomLeft = SumBottomLeft + pow(descriptor1(row, 0), 2);
195 |                     SumBottomRight = SumBottomRight + pow(descriptor2(row, 0), 2);
196 |                 }
197 |                 float SumBottom = sqrt(SumBottomLeft*SumBottomRight);
198 |                 score = SumTop/SumBottom;
199 |                 scoreMatrix(i, j) = abs(score);
200 |             }
201 |         }
202 |         //cout<<scoreMatrix<<endl;
203 | 
204 |     }
205 | }
206 | 
207 | matcher::~matcher(){
208 |     cout<<"The matching object destroyed"<<endl;
209 | }
210 | 
211 | MatrixXi matcher::getMatcherID(){
212 |     MatrixXi matcherID(size1, 2);
213 |     for(int i=0; i < size1; i++){
214 |         matcherID(i, 0) = i;
215 |         float maxScore = 0;
216 |         int ID = 1000;
217 |         for(int j=0; j<size2; j++){
218 |             if(scoreMatrix(i, j)>maxScore){
219 |                 maxScore = scoreMatrix(i, j);
220 |                 ID = j;
221 |             }
222 |         }
223 |         matcherID(i, 1) = ID;
224 |     }
225 |     return matcherID;
226 | }
227 | 
228 | MatrixXi matcher::getGoodMatcher(){
229 |     MatrixXi matcherID(size1, 3);
230 |     MatrixXi goodMatcherId;
231 |     matcherID.setZero();
232 |     int count = 0;
233 |     for(int i =0; i<size1; i++){
234 |         //cout<<"current point: "<<i<<" -------------------"<<endl;
235 |         int ID = 1000;
236 |         float maxScore = 0;
237 |         float secondMaxScore = 0;
238 |         if(noNei(i) == 1){
239 |             continue;
240 |         }
241 |         for(int j =0; j<size2; j++){
242 |             if(scoreMatrix(i, j)>maxScore){
243 |                 secondMaxScore = maxScore;
244 |                 maxScore = scoreMatrix(i, j);
245 |                 ID = j;
246 |             }
247 | 
248 |         }
249 |         if(maxScore<0.5){
250 |             //cout<<"no good score"<<endl;
251 |             // cout<<"max: "<<maxScore<<endl;
252 |             // cout<<"secondMax: "<<secondMaxScore<<endl;
253 |             continue;
254 |         }
255 |         
256 |         int InverseID = 100000;
257 |         float InverseScore = 0;
258 |         // cout<<"matchedID: "<<ID<<endl;
259 |         for(int IDi =0; IDi<size1; IDi++){
260 |             // cout<<IDi<<endl;
261 |             // cout<<"current score: "<<scoreMatrix(IDi, ID)<<endl;
262 |             if(scoreMatrix(IDi, ID)>InverseScore){
263 |                 InverseScore = scoreMatrix(IDi, ID);
264 |                 InverseID = IDi;
265 |                 //cout<<"maxmum score: "<<InverseScore<<endl;
266 |             }
267 | 
268 |         }
269 |         if(InverseID != i){
270 |             //cout<<"no good match"<<endl;
271 |             // cout<<"ID: "<<InverseID<<endl;
272 |             // cout<<"max: "<<InverseScore<<endl;
273 |             //continue;
274 |         }
275 |         matcherID(i, 1) = ID;
276 |         matcherID(i, 2) = 1;
277 |         count = count+1;
278 |     }
279 | 
280 |     goodMatcherId.resize(count, 2);
281 |     count = 0;
282 |     for(int i =0; i<size1; i++){
283 |         if(matcherID(i, 2) != 1){
284 |             continue;
285 |         }
286 |         goodMatcherId(count, 0) = i;
287 |         goodMatcherId(count, 1) = matcherID(i, 1);
288 |         count = count+1;
289 |     }
290 |     return goodMatcherId;
291 | }
292 | 


--------------------------------------------------------------------------------
/src/registration.cpp:
--------------------------------------------------------------------------------
  1 | #include "registration.hpp"
  2 | 
  3 | registration::registration(vector<vector<float> > centerpoint1, vector<vector<float> > centerpoint2, MatrixXi matcherID){
  4 |     int size = matcherID.rows();
  5 |     sourcePoints.resize(size, 3);
  6 |     targetPoints.resize(size, 3);
  7 |     labelVector.resize(size, 1);
  8 |     totalID.resize(size, 2);
  9 |     totalID = matcherID;
 10 |     //generate the source cloud and target cloud
 11 |     for(int i=0; i<size; i++){
 12 |         int row1 = matcherID(i, 0);
 13 |         int row2 = matcherID(i, 1);
 14 | 
 15 |         sourcePoints(i, 0) = centerpoint1[row1][0];
 16 |         sourcePoints(i, 1) = centerpoint1[row1][1];
 17 |         sourcePoints(i, 2) = centerpoint1[row1][2];
 18 | 
 19 |         targetPoints(i, 0) = centerpoint2[row2][0];
 20 |         targetPoints(i, 1) = centerpoint2[row2][1];
 21 |         targetPoints(i, 2) = centerpoint2[row2][2];
 22 | 
 23 |         labelVector(i) = centerpoint1[row1][3];
 24 |     }
 25 | }
 26 | 
 27 | registration::~registration(){
 28 |     cout<<"object registration destroyed"<<endl;
 29 | }
 30 | 
 31 | void getTransform(MatrixXf source, MatrixXf target, MatrixXf& R, MatrixXf& T, int iteNum){
 32 | 
 33 |     int size = source.rows();
 34 |     MatrixXf avergae_source(1, 3);
 35 |     MatrixXf avergae_target(1, 3);
 36 |     MatrixXf converage_source(size, 3);
 37 |     MatrixXf converage_target(size, 3);
 38 |     MatrixXf A(3, 3);
 39 | 
 40 |     MatrixXf R0(3, 3);
 41 |     MatrixXf T0(3, 1);
 42 | 
 43 |     R.resize(3, 3);
 44 |     T.resize(3, 1);
 45 | 
 46 |     R.setIdentity();
 47 |     T.setZero();
 48 | 
 49 |     for(int ite=0; ite<iteNum; ite++){
 50 | 
 51 |         //initiallize some values
 52 |         avergae_source.setZero();
 53 |         avergae_target.setZero();
 54 |         converage_source.setZero();
 55 |         converage_target.setZero();
 56 |         
 57 |         //calculate the average value
 58 |         for(int i=0; i<size; i++){
 59 | 
 60 |             avergae_source(0, 0) = avergae_source(0, 0) + (source(i, 0)/size);
 61 |             avergae_source(0, 1) = avergae_source(0, 1) + (source(i, 1)/size);
 62 |             avergae_source(0, 2) = avergae_source(0, 2) + (source(i, 2)/size);
 63 | 
 64 |             avergae_target(0, 0) = avergae_target(0, 0) + (target(i, 0)/size);
 65 |             avergae_target(0, 1) = avergae_target(0, 1) + (target(i, 1)/size);
 66 |             avergae_target(0, 2) = avergae_target(0, 2) + (target(i, 2)/size);
 67 | 
 68 |         }
 69 | 
 70 |         //cout<<"source Average: "<<avergae_source<<endl;
 71 |         //cout<<"target Average: "<<avergae_target<<endl;
 72 | 
 73 |         //obtain the convaraience value of the points
 74 |         for(int i=0; i<size; i++){
 75 | 
 76 |             converage_source(i, 0) = source(i, 0) - avergae_source(0, 0);
 77 |             converage_source(i, 1) = source(i, 1) - avergae_source(0, 1);
 78 |             converage_source(i, 2) = source(i, 2) - avergae_source(0, 2);
 79 | 
 80 |             converage_target(i, 0) = target(i, 0) - avergae_target(0, 0);
 81 |             converage_target(i, 1) = target(i, 1) - avergae_target(0, 1);
 82 |             converage_target(i, 2) = target(i, 2) - avergae_target(0, 2);
 83 |         }
 84 | 
 85 |         //cout<<"source Converage: "<<converage_target<<endl;
 86 |         //cout<<"target Converage: "<<converage_target<<endl;
 87 | 
 88 |         A = converage_source.transpose() * converage_target;
 89 |         Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
 90 |         Eigen::Matrix3f V = svd.matrixV(), U = svd.matrixU();
 91 |         R0 = V * U.transpose();
 92 |         T0 = avergae_target.transpose() - R0 * (avergae_source.transpose());
 93 | 
 94 |         for (int i = 0; i < source.rows(); i++) {
 95 |             source.row(i) = (R0*source.row(i).transpose() + T0).transpose();
 96 |         }
 97 |         R = R0 * R;
 98 |         T = T0 + T;
 99 | 
100 |     }
101 | 
102 | }
103 | 
104 | void getWeightedTransform(MatrixXf source, MatrixXf target, MatrixXf& R, MatrixXf& T, VectorXf labelVector, int iteNum){
105 | 
106 |     int size = source.rows();
107 |     MatrixXf avergae_source(1, 3);
108 |     MatrixXf avergae_target(1, 3);
109 |     MatrixXf A(3, 3);
110 | 
111 |     MatrixXf R0(3, 3);
112 |     MatrixXf T0(3, 1);
113 | 
114 |     R.resize(3, 3);
115 |     T.resize(3, 1);
116 | 
117 |     R.setIdentity();
118 |     T.setZero();
119 | 
120 |     int count = 0;
121 |     for(int i =0; i<size; i++){
122 |         if(labelVector(i) == 5){
123 |             count++;
124 |         }
125 |     }
126 |     cout<<"count: "<<count<<endl;
127 | 
128 |     int new_size = size + count*10;
129 |     MatrixXf new_source(new_size, 3);
130 |     MatrixXf new_target(new_size, 3);
131 |     MatrixXf converage_source(new_size, 3);
132 |     MatrixXf converage_target(new_size, 3);
133 | 
134 |     count = 0;
135 |     for(int i =0; i<size; i++){
136 |         if(labelVector(i) == 5 ){
137 |             for(int num = 0; num<10; num++){
138 |                 int currentpose = size + count*10 + num;
139 |                 new_source(currentpose, 0) = source(i, 0);
140 |                 new_source(currentpose, 1) = source(i, 1);
141 |                 new_source(currentpose, 2) = source(i, 2);
142 | 
143 |                 new_target(currentpose, 0) = target(i, 0);
144 |                 new_target(currentpose, 1) = target(i, 1);
145 |                 new_target(currentpose, 2) = target(i, 2);
146 |             }
147 |             count++;
148 |         }     
149 |     }
150 |     for(int i =0; i<size; i++){
151 |         new_source(i, 0) = source(i, 0);
152 |         new_source(i, 1) = source(i, 1);
153 |         new_source(i, 2) = source(i, 2);
154 | 
155 |         new_target(i, 0) = target(i, 0);
156 |         new_target(i, 1) = target(i, 1);
157 |         new_target(i, 2) = target(i, 2);
158 | 
159 |     }
160 | 
161 |     for(int i =0; i<new_size; i++){
162 |         if(i < size){
163 |             cout<<"source: "<<source(i, 0)<<" "<<source(i, 1)<<" "<<source(i, 2)<<endl;
164 |             cout<<"target: "<<target(i, 0)<<" "<<target(i, 1)<<" "<<target(i, 2)<<endl;
165 |         }
166 |         cout<<"New_source: "<<new_source(i, 0)<<" "<<new_source(i, 1)<<" "<<new_source(i, 2)<<endl;
167 |         cout<<"New_target: "<<new_target(i, 0)<<" "<<new_target(i, 1)<<" "<<new_target(i, 2)<<endl;
168 |     }
169 |     for(int ite=0; ite<iteNum; ite++){
170 | 
171 |         //initiallize some values
172 |         avergae_source.setZero();
173 |         avergae_target.setZero();
174 |         converage_source.setZero();
175 |         converage_target.setZero();
176 |         
177 |         //calculate the average value
178 | 
179 |         for(int i=0; i<new_size; i++){
180 | 
181 |             avergae_source(0, 0) = avergae_source(0, 0) + (new_source(i, 0)/new_size);
182 |             avergae_source(0, 1) = avergae_source(0, 1) + (new_source(i, 1)/new_size);
183 |             avergae_source(0, 2) = avergae_source(0, 2) + (new_source(i, 2)/new_size);
184 | 
185 |             avergae_target(0, 0) = avergae_target(0, 0) + (new_target(i, 0)/new_size);
186 |             avergae_target(0, 1) = avergae_target(0, 1) + (new_target(i, 1)/new_size);
187 |             avergae_target(0, 2) = avergae_target(0, 2) + (new_target(i, 2)/new_size);
188 | 
189 |         }
190 | 
191 |         //cout<<"source Average: "<<avergae_source<<endl;
192 |         //cout<<"target Average: "<<avergae_target<<endl;
193 | 
194 |         //obtain the convaraience value of the points
195 |         for(int i=0; i<new_size; i++){
196 | 
197 |             converage_source(i, 0) = new_source(i, 0) - avergae_source(0, 0);
198 |             converage_source(i, 1) = new_source(i, 1) - avergae_source(0, 1);
199 |             converage_source(i, 2) = new_source(i, 2) - avergae_source(0, 2);
200 | 
201 |             converage_target(i, 0) = new_target(i, 0) - avergae_target(0, 0);
202 |             converage_target(i, 1) = new_target(i, 1) - avergae_target(0, 1);
203 |             converage_target(i, 2) = new_target(i, 2) - avergae_target(0, 2);
204 |         }
205 | 
206 |         //cout<<"source Converage: "<<converage_target<<endl;
207 |         //cout<<"target Converage: "<<converage_target<<endl;
208 | 
209 |         A = converage_source.transpose() * converage_target;
210 |         Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
211 |         Eigen::Matrix3f V = svd.matrixV(), U = svd.matrixU();
212 |         R0 = V * U.transpose();
213 |         T0 = avergae_target.transpose() - R0 * (avergae_source.transpose());
214 | 
215 |         for (int i = 0; i < new_source.rows(); i++) {
216 |             new_source.row(i) = (R0*new_source.row(i).transpose() + T0).transpose();
217 |         }
218 |         R = R0 * R;
219 |         T = T0 + T;
220 | 
221 |     }
222 | 
223 | }
224 | 
225 | void registration::matcherRANSAC(float Td){
226 |     //generate the random points for Ransac processing
227 |     MatrixXf sourceRandom(4, 3);
228 |     MatrixXf targetRandom(4, 3);
229 |     //get the time value for obtaining the random points
230 |     clock_t startTime,endTime;
231 |     startTime = clock();
232 | 
233 |     int totalNum = sourcePoints.rows();
234 |     int maxCount = 0;
235 | 
236 |     //initialize the inlier ID
237 |     inlierID.resize(1,2);
238 |     inlierID.setZero();
239 |     for(int iter=0; iter<1000; iter++){
240 |         endTime = clock();
241 |         int timedifference = (int)((endTime - startTime)*10000);
242 |         srand((int)time(0)+timedifference);
243 | 
244 |         //obtain the random points
245 |         int Count = sourcePoints.rows();
246 |         for(int i=0; i<4; i++){
247 |             int randomNum = rand()%Count;
248 |             sourceRandom.row(i) = sourcePoints.row(randomNum);
249 |             targetRandom.row(i) = targetPoints.row(randomNum);
250 |         }
251 | 
252 |         //calculate the R and T
253 |         MatrixXf R, T;
254 |         getTransform(sourceRandom, targetRandom, R, T, 1);
255 |         // cout<<"R:"<<endl;
256 |         // cout<<R<<endl;
257 |         // cout<<"T: "<<endl;
258 |         // cout<<T<<endl;
259 | 
260 |         //find the inliers
261 |         int inliersCount = 0;
262 |         vector<int> inliers;
263 |         for(int i=0; i<sourcePoints.rows(); i++){
264 |             MatrixXf newSource(1, 3);
265 |             MatrixXf newDifference(1, 3);
266 |             newSource = (R*sourcePoints.row(i).transpose() + T).transpose();
267 |             //calculate the distance
268 |             newDifference = targetPoints.row(i) - newSource;
269 |             float distance = sqrt(pow(newDifference(0, 0), 2) + pow(newDifference(0, 1), 2) + pow(newDifference(0, 2), 2));
270 |             if(distance<Td){
271 |                 inliersCount = inliersCount + 1;
272 |                 inliers.push_back(i);
273 |             }
274 |         }
275 | 
276 |         if(inliersCount<=maxCount){
277 |             continue;   
278 |         }
279 |         //get the new best solusion
280 |         maxCount = inliersCount;
281 |     
282 |         //if the inliers are not enough, continue search
283 |         if(inliersCount<0.01*totalNum || inliersCount<5){
284 |             continue;
285 |         }
286 | 
287 |         inlierID.setZero();
288 |         inlierID.resize(inliersCount,2);
289 |         inlierTotalID.setZero();
290 |         inlierTotalID.resize(inliersCount);
291 |         for(int i=0; i<inliersCount; i++){
292 |             int oldID = inliers[i];
293 |             inlierTotalID(i) = oldID;
294 |             inlierID(i, 0) = totalID(oldID, 0);
295 |             inlierID(i, 1) = totalID(oldID, 1);
296 |         }
297 |     }
298 |     double AllCount = (double)totalNum;
299 |     double inliCount = (double)(inlierID.rows());
300 |     double performance = inliCount/AllCount;
301 |     cout<<"inlier num: "<<inliCount<<endl;
302 |     cout<<"matching performance: "<<performance<<endl;
303 | 
304 | }
305 | 
306 | void registration::Alignment(){
307 | 
308 |     int size = inlierID.rows();
309 |     sourceInliers.resize(size, 3);
310 |     targetInliers.resize(size, 3);
311 |     for(int i=0; i<size; i++){
312 | 
313 |         int oldID = inlierTotalID(i);
314 | 
315 |         sourceInliers.row(i) = sourcePoints.row(oldID);
316 | 
317 |         targetInliers.row(i) = targetPoints.row(oldID);
318 |     }
319 | 
320 |     getTransform(sourceInliers, targetInliers, Rotation, Translation, 1000);
321 |     //getWeightedTransform(sourceInliers, targetInliers, Rotation, Translation, labelVector, 1000)
322 |     cout<<"R: "<<Rotation<<endl;
323 |     cout<<"T: "<<Translation<<endl;
324 | 
325 |     Matrix3d Rotation0;
326 |     Rotation0.setIdentity();
327 |     for(int row=0; row<3; row++){
328 |         for(int col=0; col<3; col++){
329 |             Rotation0(row,col) = (double)Rotation(row, col);
330 |         }
331 |     }
332 |     Eigen::Vector3d eulerAngle = Rotation0.eulerAngles(2, 1, 0);
333 |     eulerAngle(0) = (eulerAngle(0)/3.1415926)*180;
334 |     eulerAngle(1) = (eulerAngle(1)/3.1415926)*180;
335 |     eulerAngle(2) = (eulerAngle(2)/3.1415926)*180;
336 |     //cout<<"Eular angle: "<<eulerAngle<<endl;
337 | 
338 | }


--------------------------------------------------------------------------------
/src/toolfile.cpp:
--------------------------------------------------------------------------------
  1 | #include "toolfile.hpp"
  2 | 
  3 | void insertPose(string&dir, Mat&pose, int number){
  4 | 
  5 |     double a[number];
  6 |     double b[number];
  7 |     double c[number];
  8 |     double d[number];
  9 |     double e[number];
 10 |     double f[number];
 11 |     double g[number];
 12 | 
 13 |     string txtname = dir+"airsim.txt";
 14 |     ifstream myfile(txtname);
 15 |     int N;  
 16 |     myfile>>N;
 17 |     int count = 0;
 18 |     for(int i=0;i<number;i++) 
 19 |     {
 20 |         myfile>>a[i]>>b[i]>>c[i]>>d[i]>>e[i]>>f[i]>>g[i];
 21 |         //cout<<a[i]<<" "<<b[i]<<" "<<c[i]<<" "<<d[i]<<" "<<e[i]<<" "<<f[i]<<" "<<g[i]<<endl;
 22 |         //cout<<" "<<endl;
 23 |         pose.at<double>(i, 0) = a[i];
 24 |         pose.at<double>(i, 1) = b[i];
 25 |         pose.at<double>(i, 2) = c[i];
 26 |         pose.at<double>(i, 3) = d[i];
 27 |         pose.at<double>(i, 4) = e[i];
 28 |         pose.at<double>(i, 5) = f[i];
 29 |         pose.at<double>(i, 6) = g[i];
 30 | 
 31 |         
 32 |     } 
 33 |     myfile.close();    
 34 | }
 35 | 
 36 | void insertSYNTIAPose(string&dir, Mat&pose, int number){
 37 | 
 38 |     double a[number];
 39 |     double b[number];
 40 |     double c[number];
 41 |     double d[number];
 42 |     double e[number];
 43 |     double f[number];
 44 |     double g[number];
 45 |     double h[number];
 46 |     double i1[number];
 47 |     double j[number];
 48 |     double k[number];
 49 |     double l[number];
 50 |     double m[number];
 51 |     double n[number];
 52 |     double o[number];
 53 |     double p[number];
 54 | 
 55 |     string txtname = dir+"synthia.txt";
 56 |     ifstream myfile(txtname);
 57 |     int N;  
 58 |     myfile>>N;
 59 |     int count = 0;
 60 |     for(int i=0;i<number;i++) 
 61 |     {
 62 |         myfile>>a[i]>>b[i]>>c[i]>>d[i]>>e[i]>>f[i]>>g[i]>>h[i]>>i1[i]>>j[i]>>k[i]>>l[i]>>m[i]>>n[i]>>o[i]>>p[i];
 63 |         //cout<<a[i]<<" "<<b[i]<<" "<<c[i]<<" "<<d[i]<<" "<<e[i]<<" "<<f[i]<<" "<<g[i]<<endl;
 64 |         //cout<<" "<<endl;
 65 |         pose.at<double>(i, 0) = a[i];
 66 |         pose.at<double>(i, 1) = b[i];
 67 |         pose.at<double>(i, 2) = c[i];
 68 |         pose.at<double>(i, 3) = d[i];
 69 |         pose.at<double>(i, 4) = e[i];
 70 |         pose.at<double>(i, 5) = f[i];
 71 |         pose.at<double>(i, 6) = g[i];
 72 |         pose.at<double>(i, 7) = h[i];                
 73 |         pose.at<double>(i, 8) = i1[i];
 74 |         pose.at<double>(i, 9) = j[i];
 75 |         pose.at<double>(i, 10) = k[i];
 76 |         pose.at<double>(i, 11) = l[i];
 77 |         pose.at<double>(i, 12) = m[i];
 78 |         pose.at<double>(i, 13) = n[i];
 79 |         pose.at<double>(i, 14) = o[i];
 80 |         pose.at<double>(i, 15) = p[i];
 81 | 
 82 | 
 83 |         
 84 |     } 
 85 |     myfile.close();    
 86 | }
 87 | 
 88 | void outputCenterpoint(vector<vector<float> >& centerpoint, string txtname){
 89 |     ofstream outfile;
 90 |     outfile.open(txtname);
 91 |     int size = centerpoint.size();
 92 |     for(int i =0; i<size; i++){
 93 |         float x = centerpoint[i][0];
 94 |         float y = centerpoint[i][1];
 95 |         float z = centerpoint[i][2];
 96 |         float l = centerpoint[i][3];
 97 |         outfile<<x<<" "<<y<<" "<<z<<" "<<l<<endl;        
 98 |     }
 99 |     outfile.close();
100 | }
101 | 
102 | void inputCenterpoint(vector<vector<float> >& centerpoint, int number, string txtname){
103 |     ifstream myfile(txtname);
104 |     int N;  
105 |     myfile>>N;
106 |     int count = 0;
107 |     for(int i=0;i<number;i++) 
108 |     {
109 |         float x, y, z, l;
110 |         myfile>>x>>y>>z>>l;
111 |         vector<float> cp(4);
112 |         cp[0] = x; cp[1] = y; cp[2] = z; cp[3] = l;
113 |         cout<<x<<" "<<y<<" "<<z<<" "<<l<<endl;
114 |         cout<<" "<<endl;
115 |         centerpoint.push_back(cp);
116 |         
117 |     } 
118 |     myfile.close();    
119 | }
120 | 
121 | 
122 | cv::Mat Quaternion2Matrix (cv::Mat q)
123 | {
124 |   double w = q.at<double>(0);
125 |   double x = q.at<double>(1);
126 |   double y = q.at<double>(2);
127 |   double z = q.at<double>(3);
128 | 
129 |   double xx = x*x;
130 |   double yy = y*y;
131 |   double zz = z*z;
132 |   double xy = x*y;
133 |   double wz = w*z;
134 |   double wy = w*y;
135 |   double xz = x*z;
136 |   double yz = y*z;
137 |   double wx = w*x;
138 | 
139 |   double ret[4][4];
140 |   ret[0][0] = 1.0f-2*(yy+zz);
141 |   ret[0][1] = 2*(xy-wz);
142 |   ret[0][2] = 2*(wy+xz);
143 |   ret[0][3] = 0.0f;
144 |  
145 |   ret[1][0] = 2*(xy+wz);
146 |   ret[1][1] = 1.0f-2*(xx+zz);
147 |   ret[1][2] = 2*(yz-wx);
148 |   ret[1][3] = 0.0f;
149 |  
150 |   ret[2][0] = 2*(xz-wy);
151 |   ret[2][1] = 2*(yz+wx);
152 |   ret[2][2] = 1.0f-2*(xx+yy);
153 |   ret[2][3] = 0.0f;
154 |  
155 |   ret[3][0] = 0.0f;
156 |   ret[3][1] = 0.0f;
157 |   ret[3][2] = 0.0f;
158 |   ret[3][3] = 1.0f;
159 |  
160 |   return cv::Mat(4,4,CV_64FC1,ret).clone();
161 | }
162 | 
163 | Matrix4f obtainTransformMatrix(Mat pose, int i, int type){
164 |     Matrix4f frame_pose = Matrix4f::Identity();
165 |     if(type == 1){
166 |         Mat q = cv::Mat::zeros(4, 1, CV_64FC1);
167 |         q.at<double>(0,0) = pose.at<double>(i, 3);
168 |         q.at<double>(0,1) = pose.at<double>(i, 4);
169 |         q.at<double>(0,2) = pose.at<double>(i, 5);
170 |         q.at<double>(0,3) = pose.at<double>(i, 6);
171 |         Mat rotation = cv::Mat::zeros(4, 4, CV_64FC1);
172 |         rotation = Quaternion2Matrix (q);
173 |         for(int row = 0; row<3; row++){
174 |             for(int col=0; col<3; col++){
175 |                 frame_pose(row, col) = rotation.at<double>(row, col);
176 |             }
177 |         }
178 |         frame_pose(0,3) = pose.at<double>(i, 0);
179 |         frame_pose(1,3) = pose.at<double>(i, 1);
180 |         frame_pose(2,3) = pose.at<double>(i, 2);
181 |     }
182 |     else{
183 | 
184 |         int count = 0;
185 |         for(int col = 0; col<4; col++){
186 |             for(int row=0; row<4; row++){
187 |                 frame_pose(row, col) = pose.at<double>(i, count);
188 |                 count = count + 1;
189 |             }
190 |         }
191 |         
192 |     }
193 |     return frame_pose;
194 | }
195 | 
196 | void gatherPointCloudData(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, vector<vector<float> >& centerpoint, Mat pose, Mat Label, vector<uchar> label_gray, vector<float> camera, float scale, string dir, int fileNumber, int startpoint){
197 |     //init the pointcloud mapping structure 
198 |     pointCloudMapping pointCloudMapping;
199 |     //initiallize the frame pose
200 |     Matrix4f frame_pose = Matrix4f::Identity();
201 |     Matrix4f frame_pose0 = Matrix4f::Identity();
202 |     Matrix4f camera_pose = Matrix4f::Identity();
203 | 
204 |     for(int i =startpoint; i<fileNumber; i++){
205 |         cout<<"number: "<<i<<endl;
206 |         if(i%3 != 0 && i !=0){
207 |             //continue;
208 |         }
209 | 
210 |         //load the pose of the current image
211 |         frame_pose = obtainTransformMatrix(pose, i, 1);
212 |         
213 |         if(i == startpoint){
214 |             frame_pose0 = frame_pose;
215 |         }
216 |         camera_pose = frame_pose0.inverse() * frame_pose;
217 | 
218 |         //load the rgb image, depth image and the segmentation image
219 |         char base_name[256];
220 |         sprintf(base_name,"%d.png",i);
221 |         string depth_file_name =dir+"depth/depth_" + base_name;
222 |         string rgb_file = dir+"segmentation/segmentation_"+base_name;
223 |         //only SYNTHIA need
224 |         //string label_file = dir+"label/segmentation_"+base_name;
225 | 
226 |         Mat depth = imread(depth_file_name,0);
227 |         Mat image_rgb = imread(rgb_file);
228 |         //only SYNTHIA need
229 |         //Mat lab = imread(label_file,0);
230 | 
231 |         //3D recontrcute the points
232 |         pointCloudMapping.insertValue(cloud, depth, image_rgb, frame_pose, camera, scale, centerpoint, Label);
233 |         pointCloudMapping.pointExtraction(label_gray);
234 |         centerpoint = pointCloudMapping.Cpoint;    
235 |         cloud = pointCloudMapping.outputPointCloud();
236 |         //viewer.showCloud(cloud);
237 |         waitKey(1);    
238 |     }
239 | 
240 | }
241 | 
242 | void gatherDenseMap(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, vector<vector<float> >& centerpoint, Mat pose, Mat Label, vector<float> camera, float scale, string dir, int fileNumber, int type){
243 |     //init the pointcloud mapping structure 
244 |     pointCloudMapping pointCloudMapping;
245 |     //initiallize the frame pose
246 |     Matrix4f frame_pose = Matrix4f::Identity();
247 |     Matrix4f frame_pose0 = Matrix4f::Identity();
248 |     Matrix4f camera_pose = Matrix4f::Identity();
249 |     pcl::visualization::CloudViewer viewer("viewer");
250 | 
251 |     for(int i =0; i<fileNumber; i++){
252 |         if(i%10 != 0 && i !=0){
253 |             //continue;
254 |         }
255 | 
256 |         frame_pose = obtainTransformMatrix(pose, i, 1);
257 | 
258 |         if(i == 0){
259 |             frame_pose0 = frame_pose;
260 |         }
261 |         camera_pose = frame_pose0.inverse() * frame_pose;
262 |         //load the rgb image, depth image and the segmentation image
263 |         char base_name[256];
264 |         sprintf(base_name,"%d.png",i);
265 |         string depth_file_name =dir+"depth/depth_" + base_name;
266 |         string rgb_file = dir+"segmentation/segmentation_"+base_name;
267 |         cout<<depth_file_name<<endl;
268 |         Mat depth = imread(depth_file_name,0);
269 |         Mat image_rgb = imread(rgb_file);
270 |         //3D recontrcute the points
271 |         pointCloudMapping.insertValue(cloud, depth, image_rgb, frame_pose, camera, scale, centerpoint, Label);
272 |         pointCloudMapping.initialize3Dmap(type);
273 |         pointCloudMapping.pointCloudFilter();
274 |         cloud = pointCloudMapping.outputPointCloud();
275 |         viewer.showCloud(cloud);
276 |     }
277 | 
278 | }
279 | 
280 | void matchingEvaluation(vector<vector<float> > Cpoint1, vector<vector<float> > Cpoint2, MatrixXi matcherID, MatrixXi InlierID, float& Pvalue, float& Rvalue, float& Pcount1){
281 |     int size0 = matcherID.rows();
282 |     float Pcount0 = 0; float Ncount0 = 0;
283 |     for(int i = 0; i<size0; i++){
284 |         VectorXf point1(3);
285 |         VectorXf point2(3);
286 |         int ID1 = matcherID(i, 0);
287 |         int ID2 = matcherID(i, 1);
288 |         point1(0) = Cpoint1[ID1][0]; point1(1) = Cpoint1[ID1][1]; point1(2) = Cpoint1[ID1][2];
289 |         point2(0) = Cpoint2[ID2][0]; point2(1) = Cpoint2[ID2][1]; point2(2) = Cpoint2[ID2][2];
290 |         float distance = sqrt(pow(point1(0)-point2(0),2) + pow(point1(1)-point2(1),2) + pow(point1(2)-point2(2),2));
291 |         if(distance<10){
292 |             Pcount0 = Pcount0 + 1;
293 |         }
294 |         else if(distance>=10){
295 |             Ncount0 = Ncount0 + 1;
296 |         }
297 |     }
298 |     int size1 = InlierID.rows();
299 |     Pcount1 = 0; float Ncount1 = 0;    
300 |     for(int i = 0; i<size1; i++){
301 |         VectorXf point1(3);
302 |         VectorXf point2(3);
303 |         int ID1 = InlierID(i, 0);
304 |         int ID2 = InlierID(i, 1);
305 |         point1(0) = Cpoint1[ID1][0]; point1(1) = Cpoint1[ID1][1]; point1(2) = Cpoint1[ID1][2];
306 |         point2(0) = Cpoint2[ID2][0]; point2(1) = Cpoint2[ID2][1]; point2(2) = Cpoint2[ID2][2];
307 |         float distance = sqrt(pow(point1(0)-point2(0),2) + pow(point1(1)-point2(1),2) + pow(point1(2)-point2(2),2));
308 |         if(distance<10){
309 |             Pcount1 = Pcount1 + 1;
310 |         }
311 |         else if(distance>=10){
312 |             Ncount1 = Ncount1 + 1;
313 |         }
314 |     }
315 |     Pvalue = Pcount1/(Pcount1 + Ncount1);
316 |     Rvalue = Pcount1/(Pcount0);
317 |     float rate = Pcount1/(Pcount0+Ncount0);
318 |     cout<<"Pvalue: "<<Pvalue<<endl;
319 |     cout<<"Rvalue: "<<Rvalue<<endl;
320 |     cout<<"PNumber: "<<Pcount1<<endl;
321 |     cout<<"MatchRate: "<<rate<<endl;
322 | 
323 | }
324 | 
325 | void calculateGroundTruth(Mat pose1, Mat pose2, MatrixXf Rotation, MatrixXf translation, Matrix4f& T){
326 |     Matrix4f frame_pose1 = Matrix4f::Identity();
327 |     Matrix4f frame_pose2 = Matrix4f::Identity();
328 |     Matrix4f GT = Matrix4f::Identity();
329 |     Matrix4f ownPose = Matrix4f::Identity();
330 |     
331 |     frame_pose1(0,3) = pose1.at<double>(0, 0);
332 |     frame_pose1(1,3) = pose1.at<double>(0, 1);
333 |     frame_pose1(2,3) = pose1.at<double>(0, 2);
334 |     Mat q1 = cv::Mat::zeros(4, 1, CV_64FC1);
335 |     q1.at<double>(0,0) = pose1.at<double>(0, 3);
336 |     q1.at<double>(0,1) = pose1.at<double>(0, 4);
337 |     q1.at<double>(0,2) = pose1.at<double>(0, 5);
338 |     q1.at<double>(0,3) = pose1.at<double>(0, 6);
339 |     
340 |     Mat rotation1 = cv::Mat::zeros(4, 4, CV_64FC1);
341 |     rotation1 = Quaternion2Matrix (q1);
342 |     for(int row = 0; row<3; row++){
343 |         for(int col=0; col<3; col++){
344 |             frame_pose1(row, col) = rotation1.at<double>(row, col);
345 |         }
346 |     }
347 | 
348 |     frame_pose2(0,3) = pose2.at<double>(0, 0);
349 |     frame_pose2(1,3) = pose2.at<double>(0, 1);
350 |     frame_pose2(2,3) = pose2.at<double>(0, 2);
351 |     Mat q2 = cv::Mat::zeros(4, 1, CV_64FC1);
352 |     q2.at<double>(0,0) = pose2.at<double>(0, 3);
353 |     q2.at<double>(0,1) = pose2.at<double>(0, 4);
354 |     q2.at<double>(0,2) = pose2.at<double>(0, 5);
355 |     q2.at<double>(0,3) = pose2.at<double>(0, 6);
356 |     
357 |     Mat rotation2 = cv::Mat::zeros(4, 4, CV_64FC1);
358 |     rotation2 = Quaternion2Matrix (q2);
359 |     for(int row = 0; row<3; row++){
360 |         for(int col=0; col<3; col++){
361 |             frame_pose2(row, col) = rotation2.at<double>(row, col);
362 |         }
363 |     }
364 | 
365 |     GT = frame_pose1.inverse()*frame_pose2;
366 |     
367 |     for(int row = 0; row<3; row++){
368 |         for(int col=0; col<3; col++){
369 |             ownPose(row, col) = Rotation(row, col);
370 |         }
371 |     }
372 |     ownPose(0, 3) = translation(0, 0);
373 |     ownPose(1, 3) = translation(1, 0);
374 |     ownPose(2, 3) = translation(2, 0);
375 | 
376 |     T = GT * ownPose.inverse();
377 | 
378 | }
379 | 
380 | 


--------------------------------------------------------------------------------
/src/pointCloudMapping.cpp:
--------------------------------------------------------------------------------
  1 | #include "pointCloudMapping.hpp"
  2 | 
  3 | void showPointCloud(
  4 |     const vector<VectorXd, Eigen::aligned_allocator<VectorXd>> &pointcloud);
  5 | 
  6 | 
  7 | void pointCloudMapping::insertValue(pcl::PointCloud<pcl::PointXYZRGB>::Ptr&insertCloud, Mat&depthmap, Mat&image1, Matrix4f&TransformsValue, vector<float>&camera, float scale, vector<vector<float> > Centerpoint, Mat Labels){
  8 |     //cout<<"received the depth map, start generate the point cloud"<<endl;
  9 |     image = image1;
 10 |     resize(image,image,Size(),scale,scale);
 11 |     depth = depthmap;
 12 | 
 13 |     //cout<<depth<<endl;
 14 |     s = scale;
 15 |     baseline =  camera[0]*s;
 16 |     fx = camera[0]*s;
 17 |     fy = camera[1]*s; 
 18 |     cx = camera[2]*s; 
 19 |     cy = camera[3]*s;
 20 |     Cpoint = Centerpoint;
 21 |     LabelValue = Labels;
 22 |     Cloud = pcl::make_shared<pcl::PointCloud<PointXYZRGB>>( );
 23 |     GlobalCloud = pcl::make_shared<pcl::PointCloud<PointXYZRGB>>( );
 24 |     *GlobalCloud = *insertCloud;
 25 |     //cout<<"global size: "<<GlobalCloud->points.size()<<endl;
 26 |     T = TransformsValue;
 27 |     cout<<"T: "<<T<<endl;
 28 | }
 29 | 
 30 | void pointCloudMapping::initialize3Dmap(int type){
 31 |     // cloud->width = depth.cols*depth.rows;
 32 |     // cloud->height = 1;
 33 |     // cloud->points.resize (cloud->width * cloud->height);
 34 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>); 
 35 |     cout<<"type: "<<depth.type()<<endl;
 36 |     for(int i=0;i<depth.rows;i++) { 
 37 |             if(i%4 != 0){
 38 |                 continue;
 39 |             }
 40 | 
 41 |         for(int j =0; j<depth.cols; j++){
 42 |             if(j%4 != 0){
 43 |                 continue;
 44 |             }
 45 |             pcl::PointXYZRGB point;
 46 | 
 47 |             float u0= j;
 48 |             float v0= i;
 49 |             float du0=u0-cx;
 50 |             float dv0=v0-cy;
 51 |             float x0=du0/fx;
 52 |             float y0=dv0/fx;
 53 | 
 54 | 
 55 |             float Dep;
 56 |             Dep = (float(depth.at<uchar>(i, j))/255)*100;
 57 |             if(Dep > 50){
 58 |                 continue;
 59 |             }
 60 | 
 61 |             //cout<<Dep<<endl;
 62 |             float X=x0* Dep;
 63 |             float Y=y0 * Dep;
 64 |             float Z=Dep;
 65 |             
 66 |             int number = i*depth.cols+j;
 67 |             point.x = Z;
 68 |             point.y = X;
 69 |             point.z = Y;
 70 | 
 71 |             int r, g, b;
 72 |             b =  image.at<Vec3b>(i, j)[0];
 73 |             g=  image.at<Vec3b>(i, j)[1];
 74 |             r =  image.at<Vec3b>(i, j)[2];
 75 | 
 76 | 
 77 |             unsigned char R= r;
 78 |             unsigned char G= g;
 79 |             unsigned char B= b;
 80 | 
 81 |             uint8_t rp = r;  
 82 |             uint8_t gp = g; 
 83 |             uint8_t bp = b;
 84 |             uint32_t rgb = ((uint32_t )rp<<16 | (uint32_t )gp<<8 |  (uint32_t )bp);
 85 |             point.rgb = *reinterpret_cast<float*>(&rgb);
 86 | 
 87 |             temp->push_back(point);
 88 |         }
 89 |     }
 90 |     transformPointCloud (*temp, *temp, T);
 91 |     //cout<<test<<endl;
 92 |     *Cloud = *temp;
 93 |     cout<<"The original point cloud has: "<<Cloud->points.size()<<" points"<<endl;
 94 | }
 95 | 
 96 | void Transformation(float& averageX3, float& averageY3,float& averageZ3, Matrix4f T){
 97 |     
 98 |     Eigen::Vector4f point; 
 99 |     point[0] = averageX3; 
100 |     point[1] = averageY3;
101 |     point[2] = averageZ3; 
102 |     point[3] = 1;
103 |     Eigen::Vector4f pointWorld = T*point;
104 |     averageX3 = pointWorld[0];
105 |     averageY3 = pointWorld[1];
106 |     averageZ3 = pointWorld[2];
107 | 
108 | }
109 | 
110 | void Seed_Filling(const cv::Mat& binImg, cv::Mat& lableImg, cv::Mat& image, cv::Mat& depth, vector<int>& centerX, vector<int>& centerY, 
111 |                   pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp, vector<vector<float> >& Cpoint, int lab, Mat LabelValue, Matrix4f T)  
112 | {
113 |  
114 |  
115 | 	if (binImg.empty() ||
116 | 		binImg.type() != CV_8UC1)
117 | 	{
118 | 		return;
119 | 	}
120 |     
121 | 	lableImg.release();
122 | 	binImg.convertTo(lableImg, CV_32SC1);
123 | 
124 |     //cout<<lableImg.type()<<endl;
125 |     Mat imgClone = Mat(lableImg.rows + 1, lableImg.cols + 1, lableImg.type(), Scalar(0));
126 |     lableImg.copyTo(imgClone(Rect(1, 1, lableImg.cols, lableImg.rows)));
127 | 
128 |     Mat depthClone = Mat(depth.rows + 1, depth.cols + 1, depth.type(), Scalar(0));
129 |     depth.copyTo(depthClone(Rect(1, 1, depth.cols, depth.rows)));
130 |     
131 | 	int label = 20;  
132 |  
133 | 	int rows = imgClone.rows - 1;  
134 | 	int cols = imgClone .cols - 1;
135 | 	for (int i = 1; i < rows-1; i++)
136 | 	{
137 | 		int* data= imgClone.ptr<int>(i);
138 | 		for (int j = 1; j < cols-1; j++)
139 | 		{
140 | 			if (data[j] == 255)
141 | 			{
142 | 				std::stack<std::pair<int,int>> neighborPixels;   
143 | 				neighborPixels.push(std::pair<int,int>(i,j)); 
144 | 				label = label + 1; 
145 |                 if(label == 255){
146 |                     label = label+1;
147 |                 }
148 |                 int count = 0;
149 |                 float sumX = 0;
150 |                 float sumY = 0;
151 |                 float sum3dX = 0;
152 |                 float sum3dY = 0;
153 |                 float sum3dZ = 0;
154 |                 // cout<<"j: "<<j<<endl;
155 | 				while (!neighborPixels.empty())
156 | 				{
157 | 					std::pair<int,int> curPixel = neighborPixels.top(); 
158 | 					int curX = curPixel.first;
159 | 					int curY = curPixel.second;
160 | 					imgClone.at<int>(curX, curY) = label;
161 |  
162 | 					neighborPixels.pop();
163 |                     float Dep0, Dep1, Dep2, Dep3, Dep4;
164 |                     Dep0 = (float(depthClone.at<uchar>(curX, curY))/255)*100;
165 |                     Dep1 = (float(depthClone.at<uchar>(curX, curY-1))/255)*100;
166 |                     Dep2 = (float(depthClone.at<uchar>(curX, curY+1))/255)*100;
167 |                     Dep3 = (float(depthClone.at<uchar>(curX-1, curY))/255)*100;
168 |                     Dep4 = (float(depthClone.at<uchar>(curX+1, curY))/255)*100;
169 | 
170 | 					if (imgClone.at<int>(curX, curY-1) == 255 && abs(Dep0 - Dep1)<1)
171 | 					{
172 | 						neighborPixels.push(std::pair<int,int>(curX, curY-1));
173 | 					}
174 | 					if (imgClone.at<int>(curX, curY+1) == 255 && abs(Dep0 - Dep2)<1)
175 | 					{
176 | 						neighborPixels.push(std::pair<int,int>(curX, curY+1));
177 | 					}
178 | 					if (imgClone.at<int>(curX-1, curY) == 255 && abs(Dep0 - Dep3)<1)
179 | 					{
180 | 						neighborPixels.push(std::pair<int,int>(curX-1, curY));
181 | 					}
182 | 					if (imgClone.at<int>(curX+1, curY) == 255 && abs(Dep0 - Dep4)<1)
183 | 					{
184 | 						neighborPixels.push(std::pair<int,int>(curX+1, curY));
185 | 					}
186 |                     // if (imgClone.at<int>(curX+1, curY+1) == 255)
187 | 					// {
188 | 					// 	neighborPixels.push(std::pair<int,int>(curX+1, curY+1));
189 | 					// }
190 |                     // if (imgClone.at<int>(curX+1, curY-1) == 255)
191 | 					// {
192 | 					// 	neighborPixels.push(std::pair<int,int>(curX+1, curY-1));
193 | 					// }
194 |                     // if (imgClone.at<int>(curX-1, curY-1) == 255)
195 | 					// {
196 | 					// 	neighborPixels.push(std::pair<int,int>(curX-1, curY-1));
197 | 					// }
198 |                     // if (imgClone.at<int>(curX-1, curY+1) == 255)
199 | 					// {
200 | 					// 	neighborPixels.push(std::pair<int,int>(curX-1, curY+1));
201 | 					// }
202 | 
203 |                     //generate 2D center
204 |                     float u0 = (float)curY;
205 |                     float v0 = (float)curX;
206 |                     u0 = u0-1;
207 |                     v0 = v0-1;
208 |                     sumX = sumX + u0;
209 |                     sumY = sumY + v0;
210 |                     // if(count >8070 && count<8100){
211 |                     //     cout<<"count: "<<count<<endl;
212 |                     //     cout<<"Y: "<<curY<<endl;
213 |                     // }
214 | 
215 |                     float du0=u0-512;
216 |                     float dv0=v0-288;
217 |                     float x0=du0/512;
218 |                     float y0=dv0/512;
219 | 
220 |                     float Dep;
221 |                     Dep = (float(depthClone.at<uchar>(curX, curY))/255)*100;
222 |                     if(Dep > 30){
223 |                         continue;
224 |                     }
225 |                     count = count + 1;
226 |                     //cout<<Dep<<endl;
227 |                     float X=x0* Dep;
228 |                     float Y=y0 * Dep;
229 |                     float Z=Dep;
230 |                     sum3dX = sum3dX + X;
231 |                     sum3dY = sum3dY + Y;
232 |                     sum3dZ = sum3dZ + Z;
233 | 
234 | 				}
235 |                 if(count >5000){
236 |                     float averageXf = sumX / count;
237 |                     float averageYf = sumY / count;
238 |                     int aveX = (int)averageXf;
239 |                     int aveY = (int)averageYf;
240 |                     //cout<<"aveX: "<<aveX<<endl;
241 |                     //cout<<"aveY: "<<aveY<<endl;
242 |                     centerX.push_back(aveX);
243 |                     centerY.push_back(aveY);
244 |                     //point
245 |                     float averageX3 = sum3dZ / count;
246 |                     float averageY3 = sum3dX / count;
247 |                     float averageZ3 = sum3dY / count;
248 |                     pcl::PointXYZRGB point;
249 |                     point.x = averageX3;
250 |                     point.y = averageY3;
251 |                     point.z = averageZ3;
252 |                     //cout<<"Z: "<<point.z<<endl;
253 |                     int b =  LabelValue.at<int>(lab, 2);
254 |                     int g=  LabelValue.at<int>(lab, 1);
255 |                     int r =  LabelValue.at<int>(lab, 0);
256 | 
257 |                     unsigned char R= r;
258 |                     unsigned char G= g;
259 |                     unsigned char B= b;
260 | 
261 |                     uint8_t rp = r;  
262 |                     uint8_t gp = g; 
263 |                     uint8_t bp = b;
264 |                     uint32_t rgb = ((uint32_t )rp<<16 | (uint32_t )gp<<8 |  (uint32_t )bp);
265 |                     point.rgb = *reinterpret_cast<float*>(&rgb);
266 | 
267 |                     vector<float> cp(4);
268 |                     Transformation(averageX3, averageY3, averageZ3, T);
269 |                     cp[0] = averageX3; cp[1] = averageY3; cp[2] = averageZ3; cp[3] = (float)lab;
270 |                     int CSzie = Cpoint.size();
271 |                     //cout<<"Cpoint size: "<<CSzie<<endl;
272 |                     if(CSzie>0){
273 |                         bool fuse = false;
274 |                         float Distance = 10000;
275 |                         for(int sizeI = 0; sizeI<CSzie; sizeI++){
276 |                             float previousLab = Cpoint[sizeI][3];
277 |                             if(cp[3] == previousLab){
278 |                                 Distance = sqrt(pow(Cpoint[sizeI][0] - cp[0],2) + pow(Cpoint[sizeI][1] - cp[1],2) + pow(Cpoint[sizeI][2] - cp[2],2));
279 |                                 //cout<<"distance: "<<Distance<<endl;
280 |                                 if(Distance<5 && lab>7){
281 |                                     // cout<<Cpoint[sizeI][2]<<endl;
282 |                                     // cout<<cp[2]<<endl;
283 |                                     //cout<<"______________Road point fuse________________"<<endl;
284 |                                     fuse = true;
285 |                                 }
286 |                                 else if(Distance<5 && lab<8){
287 |                                     // Cpoint[sizeI][0] = (Cpoint[sizeI][0] + cp[0])/2;
288 |                                     // Cpoint[sizeI][1] = (Cpoint[sizeI][1] + cp[1])/2;
289 |                                     // Cpoint[sizeI][2] = (Cpoint[sizeI][2] + cp[2])/2;
290 |                                     //cout<<"______________Object point fuse________________"<<endl;
291 |                                     fuse = true;
292 |                                 }
293 |                             }
294 |                         }
295 |                         if(fuse == true){
296 |                             continue;
297 |                         }
298 |                     }
299 |                     temp->push_back(point);
300 |                     Cpoint.push_back(cp);
301 | 
302 |                 }		
303 | 			}
304 | 		}
305 | 	}
306 | 	
307 | }
308 | 
309 | void pointCloudMapping::pointExtraction(vector<uchar> label){
310 | 
311 |     Mat image_gray;
312 |     vector<int> centerX;
313 |     vector<int> centerY;
314 |     cvtColor(image, image_gray, CV_BGR2GRAY);
315 |     int imgRow = image.rows; int imgCol = image.cols;
316 |     //cout<<image_gray.row(270)<<endl;
317 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>); 
318 |     for(int lab = 0; lab<11; lab++){
319 |         Mat image_bw = cv::Mat::zeros(imgRow, imgCol, CV_8UC1);
320 |         for(int row = 0; row<imgRow; row++){
321 |             for(int col = 0; col<imgCol; col++){
322 |                 if(image_gray.at<uchar>(row, col) == label[lab]){
323 |                     image_bw.at<uchar>(row, col) = 255;
324 |                 }
325 |                 else{
326 |                     image_bw.at<uchar>(row, col) = 0;
327 |                 }
328 | 
329 |             }
330 |         }
331 |         cv::Mat labelImg;
332 |         // /cout<<"label: "<<lab<<endl;
333 |         Seed_Filling(image_bw, labelImg, image, depth, centerX, centerY, temp, Cpoint, lab, LabelValue, T);
334 |         //cout<<centerX.size()<<endl;
335 |         for(int sizeNum = 0; sizeNum<centerX.size(); sizeNum++){
336 |             Point p1;
337 |             p1.x = centerX[sizeNum];
338 |             p1.y = centerY[sizeNum];
339 |             circle(image, p1, 15,Scalar(255,255,0),3);
340 |         }
341 |         // imshow("bbb", image_bw);
342 |         // waitKey(2000);
343 | 
344 |     }
345 |     imshow("aaa", image);
346 |     transformPointCloud (*temp, *temp, T);
347 |     *Cloud = *temp;
348 |     cout<<"The number of keypoint is: "<<Cloud->points.size()<<" points"<<endl;    
349 | }
350 | 
351 | void pointCloudMapping::pointExtraction(vector<uchar> label, Mat labelimag){
352 |     cout<<labelimag.type()<<endl;
353 |     vector<int> centerX;
354 |     vector<int> centerY;
355 |     int imgRow = image.rows; int imgCol = image.cols;
356 | 
357 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>); 
358 |     for(int lab = 0; lab<9; lab++){
359 |         Mat image_bw = cv::Mat::zeros(imgRow, imgCol, CV_8UC1);
360 |         for(int row = 0; row<imgRow; row++){
361 |             for(int col = 0; col<imgCol; col++){
362 |                 if(labelimag.at<uchar>(row, col) == label[lab]){
363 |                     image_bw.at<uchar>(row, col) = 255;
364 |                 }
365 |                 else{
366 |                     image_bw.at<uchar>(row, col) = 0;
367 |                 }
368 |             }
369 |         }
370 |         cv::Mat labelImg;
371 |         // /cout<<"label: "<<lab<<endl;
372 |         Seed_Filling(image_bw, labelImg, image, depth, centerX, centerY, temp, Cpoint, lab, LabelValue, T);
373 |         //cout<<centerX.size()<<endl;
374 |         for(int sizeNum = 0; sizeNum<centerX.size(); sizeNum++){
375 |             Point p1;
376 |             p1.x = centerX[sizeNum];
377 |             p1.y = centerY[sizeNum];
378 |             circle(image, p1, 15,Scalar(255,255,0),3);
379 |         }
380 |         // imshow("bbb", image_bw);
381 |         // waitKey(2000);
382 | 
383 |     }
384 |     imshow("aaa", image);
385 |     transformPointCloud (*temp, *temp, T);
386 |     *Cloud = *temp;
387 |     cout<<"The number of keypoint is: "<<Cloud->points.size()<<" points"<<endl;    
388 | }
389 | 
390 | void pointCloudMapping::pointCloudFilter(){
391 | 
392 |     pcl::VoxelGrid<pcl::PointXYZRGB> downSampled;  
393 |     downSampled.setInputCloud (Cloud);            
394 |     downSampled.setLeafSize (1.8f, 1.8f, 1.8f);  
395 |     downSampled.setDownsampleAllData(true);
396 |     downSampled.filter (*Cloud);
397 |               
398 |     // pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> pcFilter;  
399 |     // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);    
400 |     // pcFilter.setInputCloud(Cloud);             
401 |     // pcFilter.setRadiusSearch(200);        
402 |     // pcFilter.setMinNeighborsInRadius(4);      
403 |     // pcFilter.filter(*Cloud);       
404 | 
405 |     cout<<"The filtered point cloud has: "<<Cloud->points.size()<<" points"<<endl;
406 |         
407 | }
408 | 
409 | pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudMapping::outputPointCloud()
410 | {
411 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>); 
412 |     *temp = *GlobalCloud+*Cloud;
413 |     return temp;
414 | }
415 | 
416 | void showPointCloud(const vector<VectorXd, Eigen::aligned_allocator<VectorXd>> &pointcloud) {
417 | 
418 |     if (pointcloud.empty()) {
419 |         cerr << "Point cloud is empty!" << endl;
420 |         return;
421 |     }
422 |     //创建一个窗口
423 |     pangolin::CreateWindowAndBind("Point Cloud Viewer_1", 1024, 768);
424 |     //启动深度测试
425 |     glEnable(GL_DEPTH_TEST);
426 |     glEnable(GL_BLEND);
427 |     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
428 |     pangolin::DataLog log;
429 |     std::vector<std::string> labels;
430 |     labels.push_back(std::string("sin(t)"));
431 |     log.SetLabels(labels);
432 |     // Define Projection and initial ModelView matrix
433 |     pangolin::OpenGlRenderState s_cam(
434 |         pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
435 |         pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
436 |     );
437 | 
438 |     pangolin::View &d_cam = pangolin::CreateDisplay()
439 |         .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
440 |         .SetHandler(new pangolin::Handler3D(s_cam));
441 | 
442 |     while (pangolin::ShouldQuit() == false) {
443 |         // Clear screen and active view to render into
444 |         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
445 |         pangolin::glDrawAxis(3);
446 |         d_cam.Activate(s_cam);
447 |         glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
448 | 
449 |         glPointSize(20);
450 |         glBegin(GL_POINTS);
451 |         for (auto &p: pointcloud) {
452 |             glColor3f(p[3], p[4], p[5]);
453 |             glVertex3d(p[0], p[1], p[2]);
454 |         }
455 |         glEnd();
456 |         // Swap frames and Process Events
457 |         pangolin::FinishFrame();
458 |         usleep(5000);   // sleep 5 ms
459 |     }
460 |     return;
461 | }
462 | 
463 | void pointCloudMapping::pointVisuallize(pcl::PointCloud<pcl::PointXYZRGB>::Ptr&insertCloud1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr&insertCloud2, MatrixXi matcherID, MatrixXf R, MatrixXf T){
464 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGB>);
465 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp1(new pcl::PointCloud<pcl::PointXYZRGB>);
466 |     pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp2(new pcl::PointCloud<pcl::PointXYZRGB>);
467 |     //obtain the clouds.
468 |     Matrix4f TransforMatrix = Matrix4f::Identity();
469 |     // for(int row = 0; row<3; row++){
470 |     //     for(int col=0; col<3; col++){
471 |     //         TransforMatrix(row, col) = R(row, col);
472 |     //     }
473 |     // }
474 |     // TransforMatrix(0, 3) = T(0, 0);
475 |     // TransforMatrix(1, 3) = T(1, 0);
476 |     // TransforMatrix(2, 3) = T(2, 0);
477 | 
478 |     TransforMatrix(2, 3) = TransforMatrix(2, 3)+80;  
479 |     *temp1 = *insertCloud1;
480 |     *temp2 = *insertCloud2;
481 |     transformPointCloud (*temp1, *temp1, TransforMatrix);
482 |     *temp = *temp1 + *temp2;
483 |     //obtain the line information
484 |     int size = matcherID.rows();
485 | 
486 |     //cout<<" has: "<<temp->points.size()<<" points"<<endl;
487 |     boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("point cloud"));
488 |     viewer->setBackgroundColor(0, 0, 0);
489 |     pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(temp);
490 |     viewer->addPointCloud<pcl::PointXYZRGB>(temp, rgb, "Point cloud");
491 |     viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 25, "Point cloud");
492 |     viewer->addCoordinateSystem(1.0);
493 |     viewer->initCameraParameters();
494 |     viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0);
495 | 
496 |     int count = 0;
497 |     int line_numeber = 0;
498 |     char str[25];//
499 |     while(!viewer->wasStopped()){
500 |         viewer->spinOnce(1000);
501 |         line_numeber++;
502 |         sprintf(str, "%d", line_numeber);
503 |         if(count < size){
504 |             viewer->addLine<pcl::PointXYZRGB> (temp1->points[matcherID(count, 0)], temp2->points[matcherID(count, 1)], str);
505 |         }
506 |         count++;
507 | 
508 |     }
509 | 
510 |     // vector<VectorXd, Eigen::aligned_allocator<VectorXd>> pointcloud;
511 |     // cout<<"global size: "<<insertCloud->points.size()<<endl;
512 |     // for(int i = 0; i<insertCloud->points.size(); i++){
513 |     //         int r = insertCloud->points[i].r;
514 |     //         int g = insertCloud->points[i].g;
515 |     //         int b = insertCloud->points[i].b;
516 |     //         Eigen::VectorXd point1(6);
517 |     //         point1<< 0, 0, 0, r/ 255.0, g/255.0, b/255.0;
518 |     //         point1[0] = double(insertCloud->points[i].x);
519 |     //         //cout << "X" << " " << point1[0]  << endl;
520 |     //         point1[1] = double(insertCloud->points[i].y);
521 |     //         //cout << "Y" << " " << point1[1] << endl;
522 |     //         point1[2] = double(insertCloud->points[i].z);
523 |     //         //cout << "Z" << " " << point1[2] << endl;
524 |     //         pointcloud.push_back(point1);
525 |     // }
526 |     // showPointCloud(pointcloud);
527 | }


--------------------------------------------------------------------------------