Statistics
| Branch: | Revision:

root / rgbdslam / src / gicp-fallback.cpp @ 9240aaa3

History | View | Annotate | Download (5.05 KB)

1
/* This file is part of RGBDSLAM.
2
 * 
3
 * RGBDSLAM is free software: you can redistribute it and/or modify
4
 * it under the terms of the GNU General Public License as published by
5
 * the Free Software Foundation, either version 3 of the License, or
6
 * (at your option) any later version.
7
 * 
8
 * RGBDSLAM is distributed in the hope that it will be useful,
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
 * GNU General Public License for more details.
12
 * 
13
 * You should have received a copy of the GNU General Public License
14
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
15
 */
16

    
17

    
18
 /*
19
 * gicp.cpp
20
 *
21
 *  Created on: Jan 23, 2011
22
 *      Author: engelhar
23
 */
24

    
25
#include "gicp-fallback.h"
26

    
27
#include <fstream>
28
#include <sstream>
29
#include <iostream>
30
#include <pcl/filters/voxel_grid.h>
31
using namespace std;
32

    
33
void saveCloud(const char* filename, const PointCloud_RGB& pc, const int max_cnt, const bool color){
34

    
35
    ofstream of;
36
    of.open(filename);
37
    assert(of.is_open());
38

    
39
    
40
    int write_step = 1;
41
    if (max_cnt>0 && (int)pc.points.size()>max_cnt)
42
        write_step = floor(pc.points.size()*1.0/max_cnt);
43
    
44
    int cnt = 0;
45
    assert(write_step > 0);
46

    
47
    
48
    // only write every write_step.th points
49
    for (unsigned int i=0; i<pc.points.size(); i += write_step)
50
    {
51
        Point p = pc.points[i];
52

    
53
        bool invalid = (isnan(p.x) || isnan(p.y) || isnan(p.z));
54
        if (invalid)
55
                continue;
56

    
57
        
58
        of << p.x << "\t" << p.y << "\t" << p.z;
59
        if (color) {
60
                
61
                int color = *reinterpret_cast<const int*>(&p.rgb); 
62
                int r = (0xff0000 & color) >> 16;
63
                int g = (0x00ff00 & color) >> 8;
64
                int b = 0x0000ff & color; 
65
                of << "\t \t" << r << "\t" << g << "\t" << b << "\t" << endl;
66
        }        
67
        else
68
                of << endl;
69
       // cout << p.x << "\t" << p.y << "\t" << p.z << endl;
70
                       
71
        cnt++;
72
    }
73
    
74

    
75
   // ROS_INFO("gicp.cpp:  saved %i pts (of %i) to %s", cnt,(int) pc.points.size(), filename);
76
   // printf("gicp.cpp:  saved %i pts (of %i) to %s \n", cnt,(int) pc.points.size(), filename);
77

    
78
    of.close();
79

    
80
}
81

    
82

    
83
void downSample(const PointCloud_RGB& src, PointCloud_RGB& to){
84
    pcl::VoxelGrid<Point> down_sampler;
85
    down_sampler.setLeafSize (0.01, 0.01, 0.01);
86
    pcl::PCLBase<Point>::PointCloudConstPtr const_cloud_ptr = boost::make_shared<PointCloud_RGB> (src);
87
    down_sampler.setInputCloud (const_cloud_ptr);
88
    down_sampler.filter(to);
89
    ROS_INFO("gicp.cpp: Downsampling from %i to %i", (int) src.points.size(), (int) to.points.size());
90
}
91

    
92

    
93

    
94
bool gicpfallback(const PointCloud_RGB& from, const PointCloud_RGB& to, Eigen::Matrix4f& transform){
95

    
96
        // std::clock_t starttime_gicp = std::clock();
97
        
98
    FILE *fp;
99
    char f1[200];
100
    char f2[200];
101

    
102
    char line[130];
103
    char cmd[200];
104

    
105
    sprintf(f1, "pc1.txt");
106
    sprintf(f2, "pc2.txt");
107

    
108
    // default values for algo work well on this data
109
    sprintf(cmd, "./gicp/test_gicp %s %s --d_max 0.01  --debug 0",f1,f2); 
110

    
111
    int N = 40000;
112
    
113
    saveCloud(f1,from,N);
114
    saveCloud(f2,to,N);
115

    
116
    // cout << "time for writing: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC) << endl;
117
    // std::clock_t starttime_gicp2 = std::clock();
118
    
119
    /*
120
     ICP is calculated by external program. It writes some intermediate results
121
     and the final homography on stdout, which is parsed here.
122
     Not very beautiful, but works :)
123
     */
124
    fp = popen(cmd, "r");
125

    
126
    std::vector<string> lines;
127

    
128
    // collect all output
129
    while ( fgets( line, sizeof line, fp))
130
    {
131
        lines.push_back(line);
132
        // ROS_INFO("gicp.cpp: %s", line);
133
    }
134
    int retval = pclose(fp);
135
   // cout << "time for binary: " << ((std::clock()-starttime_gicp2*1.0) / (double)CLOCKS_PER_SEC) << endl;
136

    
137
   // std::clock_t starttime_gicp3 = std::clock();
138
        
139
    if(retval != 0){
140
        ROS_ERROR_ONCE("Non-zero return value from %s: %i. Identity transformation is returned instead of GICP result.\nThis error will be reported only once.", cmd, retval);
141
        transform = Eigen::Matrix<float, 4, 4>::Identity();
142
        return false;
143
    }
144

    
145
    int pos = 0;
146
    // last lines contain the transformation:
147
    for (unsigned int i=lines.size()-5; i<lines.size()-1; i++){
148
            
149
        stringstream ss(lines.at(i));
150
        for (int j=0; j<4; j++)
151
        {
152
            ss >> line;
153
            transform(pos,j) = atof(line);
154
        }
155
        // cout << endl;
156
        pos++;
157
    }
158
    
159
    // read the number of iterations:
160
    stringstream ss(lines.at(lines.size()-1));
161
    ss >> line; // Converged in
162
    ss >> line;
163
    ss >> line;
164
        
165
    int iter_cnt;
166
    iter_cnt = atoi(line);
167
    
168
    return iter_cnt < 200; // check test_gicp for maximal allowed number of iterations
169
    
170
    // ROS_INFO_STREAM("Parsed: Converged in " << iter_cnt << "iterations");
171
    
172
    
173
    // ROS_DEBUG_STREAM("Matrix read from ICP process: " << transform);
174
    // ROS_INFO_STREAM("Paper: time for icp1 (internal): " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC));
175
}