Skip to content

Commit 6f6b3eb

Browse files
junhaoxiaojunhaoxiao
committed
initialization of the code
1 parent fa07697 commit 6f6b3eb

File tree

170 files changed

+30570
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

170 files changed

+30570
-0
lines changed

‎CMakeLists.txt

+62
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
cmake_minimum_required(VERSION 2.8)
2+
project(tams)
3+
4+
if(POLICY CMP0017)
5+
cmake_policy(SET CMP0017 NEW)
6+
endif()
7+
set(CMAKE_BUILD_TYPE Release)
8+
9+
#find packages
10+
list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
11+
12+
set(WORKSPACE /home/xiao/workspace/pcl)
13+
set(SOFT /home/xiao/workspace/soft)
14+
15+
16+
#cgal package
17+
find_package(CGAL QUIET COMPONENTS Core )
18+
if ( CGAL_FOUND )
19+
include( ${CGAL_USE_FILE} )
20+
include( CGAL_CreateSingleSourceCGALProgram )
21+
include_directories (BEFORE include)
22+
else()
23+
message(STATUS "This program requires the CGAL library, and will not be compiled.")
24+
endif()
25+
26+
#pcl package
27+
find_package(PCL 1.6)
28+
include_directories(${PCL_INCLUDE_DIRS})
29+
link_directories(${PCL_LIBRARY_DIRS})
30+
add_definitions(${PCL_DEFINITIONS})
31+
32+
#opencv
33+
FIND_PACKAGE( OpenCV REQUIRED )
34+
#Eigen
35+
#find_package(Eigen REQUIRED)
36+
#include_directories(${EIGEN_INCLUDE_DIRS})
37+
#SOFT
38+
link_directories(${SOFT})
39+
include_directories(${SOFT}/include)
40+
41+
42+
FILE(GLOB_RECURSE new_list *.h)
43+
SET(dir_list "")
44+
FOREACH(file_path ${new_list})
45+
GET_FILENAME_COMPONENT(dir_path ${file_path} PATH)
46+
SET(dir_list ${dir_list} ${dir_path}/../../include)
47+
ENDFOREACH()
48+
LIST(REMOVE_DUPLICATES dir_list)
49+
INCLUDE_DIRECTORIES(${dir_list})
50+
51+
52+
file( GLOB list "*" )
53+
foreach( entry ${list} )
54+
if( IS_DIRECTORY ${entry} )
55+
if( EXISTS ${entry}/CMakeLists.txt )
56+
add_subdirectory( ${entry} )
57+
link_directories( ${entry}/lib )
58+
message( STATUS "Configuring ${entry} xiao" )
59+
endif()
60+
endif()
61+
endforeach()
62+

‎abstract_planar_segment/CMakeLists.txt

+9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
project(abstract_planar_segment)
2+
#set the default path for built executables to the "bin" directory
3+
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
4+
#set the default path for built libraries to the "lib" directory
5+
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
6+
include_directories(include)
7+
8+
add_library(abstract_planar_segment src/abstract_planar_segment.cc)
9+
target_link_libraries(abstract_planar_segment ${PCL_LIBRARIES})

‎abstract_planar_segment/include/abstract_planar_segment/abstract_planar_segment.h

+134
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Technical Aspects of Multimodal Systems (TAMS) - http://tams-www.informatik.uni-hamburg.de/
5+
*
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of TAMS, nor the names of its contributors may
19+
* be used to endorse or promote products derived from this software
20+
* without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*
35+
* Author : Junhao Xiao
36+
* Email : junhao.xiao@ieee.org, xiao@informatik.uni-hamburg.de
37+
*
38+
*/
39+
40+
#ifndef ABSTRACT_PLANAR_SEGMENT_H_
41+
#define ABSTRACT_PLANAR_SEGMENT_H_
42+
43+
//STL
44+
#include <vector>
45+
//boost
46+
#include <boost/shared_ptr.hpp>
47+
//PCL
48+
#include <pcl/point_cloud.h>
49+
#include <pcl/point_types.h>
50+
//Eigen
51+
#include <Eigen/Core>
52+
#include <Eigen/Dense>
53+
54+
#include "common/planar_patch.h"
55+
56+
namespace tams
57+
{
58+
class AbstractPlanarSegment
59+
{
60+
public:
61+
/** Typedefs. */
62+
typedef boost::shared_ptr<AbstractPlanarSegment> Ptr;
63+
typedef boost::shared_ptr<const AbstractPlanarSegment> ConstPtr;
64+
typedef std::vector<AbstractPlanarSegment, Eigen::aligned_allocator<AbstractPlanarSegment> > StdVector;
65+
typedef boost::shared_ptr<StdVector> StdVectorPtr;
66+
67+
public:
68+
/** \brief Empty constructor. */
69+
AbstractPlanarSegment():
70+
a0(0.0), a1(0.0), a2(0.0), normal(Eigen::Vector3d::Zero ()), d(0.0), weighted_center(Eigen::Vector3d::Zero ()),
71+
scatter_matrix(Eigen::Matrix3d::Zero ()), hessian(Eigen::Matrix4d::Zero()), C4x4(Eigen::Matrix4d::Zero ()), Cnn(Eigen::Matrix3d::Zero ()),
72+
Cdd(0.0), minusLogDeterminantC (0.0), minusLogDeterminantCnn (0.0), area (0.0)
73+
{
74+
75+
}
76+
77+
/** Empty deconstructor. */
78+
~AbstractPlanarSegment()
79+
{
80+
81+
}
82+
83+
/** \brief Set the noisy model of the corresponding range sensor.
84+
sensor noise delta = a0_ + a_1_ * range + a_2_ * range * range.
85+
*/
86+
void
87+
setSensorNoiseModel(double b0, double b1, double b2)
88+
{
89+
a0= b0;
90+
a1 = b1;
91+
a2 = b2;
92+
}
93+
94+
/** \brief Compute the attributes for the given segement w.r.t the corresponding point cloud. */
95+
void
96+
calculateAttributes(PlanarSegment::StdVector::iterator segment, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
97+
98+
public:
99+
/** \brief Hessian form plane equation: n.dot(p) = d. */
100+
Eigen::Vector3d normal;
101+
/** \brief Plane equation: n.dot(p) = d. */
102+
double d;
103+
/** \brief Weighted center of the segment. */
104+
Eigen::Vector3d weighted_center;
105+
/** \brief Scatter matrix of 3D points of the segment. */
106+
Eigen::Matrix3d scatter_matrix;
107+
/** \brief Hessian matrix of least square fitting. */
108+
Eigen::Matrix4d hessian;
109+
/** \brief Plane parameter covariance matrix.*/
110+
Eigen::Matrix4d C4x4;
111+
/** \brief Normal vector estimation covariance matrix.*/
112+
Eigen::Matrix3d Cnn;
113+
/** \brief Estimation variance of the distance from origin, see plane equation. */
114+
double Cdd;
115+
/** \brief minusLogDeterminantC_ = -(log(eigenvalue(C4x4_,0)) + log(eigenvalue(C4x4_,1)) + log(eigenvalue(C4x4_,2))),
116+
with the condition eigenvalue(C4x4_,3) = 0.*/
117+
double minusLogDeterminantC;
118+
/** \brief minusLogDeterminantCnn_ = -(log(eigenvalue(Cnn_,0)) + log(eigenvalue(Cnn_,1))),
119+
with the condition eigenvalue(C4x4_,2) = 0.*/
120+
double minusLogDeterminantCnn;
121+
/** \brief Area covered by the points on the segment. */
122+
double area;
123+
/** \brief Number of points in this segment. */
124+
size_t point_num;
125+
/** \brief sensor noise delta = a0_ + a_1_ * range + a_2_ * range * range. */
126+
double a0, a1, a2;
127+
/** \brief a metric to measure how linear (long-thin) the segment is. */
128+
double linearity;
129+
public:
130+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
131+
};
132+
}
133+
134+
#endif // ABSTRACT_PLANAR_SEGMENT_H_

‎abstract_planar_segment/src/abstract_planar_segment.cc

+180
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Technical Aspects of Multimodal Systems (TAMS) - http://tams-www.informatik.uni-hamburg.de/
5+
*
6+
* All rights reserved.
7+
*
8+
* Redistribution and use in source and binary forms, with or without
9+
* modification, are permitted provided that the following conditions
10+
* are met:
11+
*
12+
* * Redistributions of source code must retain the above copyright
13+
* notice, this list of conditions and the following disclaimer.
14+
* * Redistributions in binary form must reproduce the above
15+
* copyright notice, this list of conditions and the following
16+
* disclaimer in the documentation and/or other materials provided
17+
* with the distribution.
18+
* * Neither the name of TAMS, nor the names of its contributors may
19+
* be used to endorse or promote products derived from this software
20+
* without specific prior written permission.
21+
*
22+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33+
* POSSIBILITY OF SUCH DAMAGE.
34+
*
35+
* Author : Junhao Xiao
36+
* Email : junhao.xiao@ieee.org, xiao@informatik.uni-hamburg.de
37+
*
38+
*/
39+
40+
//Eigen
41+
#include <Eigen/Core>
42+
#include <Eigen/Eigenvalues>
43+
44+
#include <abstract_planar_segment/abstract_planar_segment.h>
45+
46+
namespace tams
47+
{
48+
void
49+
AbstractPlanarSegment::calculateAttributes(PlanarSegment::StdVector::iterator segment, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
50+
{
51+
if (a0 == 0.0 && a1 == 0.0 && a2 == 0.0)
52+
{
53+
std::cerr << "You have not set the sensor noise parameters a0_, a1_ and a2_, set them using setSensorNoiseModel(a0_, a1_, a2_)." << std::endl;
54+
return;
55+
}
56+
57+
double weight_sum;
58+
Eigen::Vector3d weighted_sum;
59+
/** the weight for each point. */
60+
std::vector<double > weights;
61+
/** the scatter matrix. */
62+
Eigen::Matrix3d S, Hnn;
63+
Eigen::Vector3d Hnd;
64+
double Hdd;
65+
66+
Eigen::EigenSolver<Eigen::Matrix4d> eigensolver4d;
67+
Eigen::Vector4d eigenvalues4f = Eigen::Vector4d::Zero();
68+
Eigen::Matrix4d eigenvectors4d = Eigen::Matrix4d::Zero();
69+
Eigen::EigenSolver<Eigen::Matrix3d> eigensolver3d;
70+
Eigen::Vector3d eigenvalues3d = Eigen::Vector3d::Zero();
71+
Eigen::Matrix3d eigenvectors3d = Eigen::Matrix3d::Zero();
72+
int min_eigenvalue_index, max_eigenvalue_index;
73+
double min_eigenvalue, max_eigenvalue;
74+
PlanarSegment::StdVector::iterator it;
75+
76+
weights.clear();
77+
weights.resize(segment->points.size());
78+
weight_sum = 0.0;
79+
weighted_sum = Eigen::Vector3d::Zero();
80+
S = Eigen::Matrix3d::Zero ();
81+
double squared_norm = 0.0;
82+
size_t i = 0;
83+
for (std::vector<int>::iterator it = segment->points.begin(); it != segment->points.end(); it++, i++)
84+
{
85+
squared_norm = cloud->points[*it].x * cloud->points[*it].x +
86+
cloud->points[*it].y * cloud->points[*it].y +
87+
cloud->points[*it].z * cloud->points[*it].z;
88+
weights[i] = a0 + a1 * sqrt(squared_norm) + a2 * squared_norm;
89+
weights[i] = 1 / (weights[i] * weights[i]);
90+
weight_sum += weights[i];
91+
weighted_sum += weights[i] * Eigen::Vector3d(cloud->points[*it].x, cloud->points[*it].y, cloud->points[*it].z);
92+
}
93+
weighted_center = weighted_sum / weight_sum;
94+
i = 0;
95+
for (std::vector<int>::iterator it = segment->points.begin(); it != segment->points.end(); it++, i++)
96+
{
97+
Eigen::Vector3d tmp(cloud->points[*it].x - weighted_center(0), cloud->points[*it].y - weighted_center(1), cloud->points[*it].z - weighted_center(2));
98+
S += weights[i] * (tmp * tmp.transpose ());
99+
}
100+
eigensolver3d.compute(S);
101+
eigenvalues3d = eigensolver3d.eigenvalues().real();
102+
eigenvectors3d = eigensolver3d.eigenvectors().real();
103+
min_eigenvalue = eigenvalues3d.minCoeff(&min_eigenvalue_index);
104+
105+
double lambada[2];
106+
i = 0;
107+
for (int j = 0; j < 3; j++)
108+
{
109+
if (j == min_eigenvalue_index)
110+
continue;
111+
lambada[i] = eigenvalues3d[j];
112+
i ++;
113+
}
114+
115+
linearity = lambada[0] / lambada[1];
116+
if (linearity < 1.0)
117+
linearity = 1/linearity;
118+
119+
Hdd = -weight_sum;
120+
Hnn = -S - weight_sum * weighted_center * weighted_center.transpose() + min_eigenvalue * Eigen::Matrix3d::Identity();
121+
Hnd = weight_sum * weighted_center;
122+
123+
hessian.block<3,3>(0,0) = Hnn;
124+
hessian.block<3,1>(0,3) = Hnd;
125+
hessian.block<1,3>(3,0) = Hnd.transpose();
126+
hessian(3,3) = Hdd;
127+
128+
eigensolver4d.compute(hessian);
129+
eigenvalues4f = eigensolver4d.eigenvalues().real();
130+
eigenvectors4d = eigensolver4d.eigenvectors().real();
131+
max_eigenvalue = eigenvalues4f.cwiseAbs().maxCoeff(&max_eigenvalue_index);
132+
min_eigenvalue = eigenvalues4f.cwiseAbs().minCoeff(&min_eigenvalue_index);
133+
134+
//std::cout << "eigenvalues: " << eigenvalues4f.transpose () << std::endl;
135+
C4x4 = Eigen::Matrix4d::Zero ();
136+
double DC4x4 = 1.0;
137+
for (int j = 0; j < 4; j++)
138+
{
139+
if (j == min_eigenvalue_index)
140+
continue;
141+
C4x4 += eigenvectors4d.col(j) * eigenvectors4d.col(j).transpose () / eigenvalues4f(j);
142+
DC4x4 /= eigenvalues4f(j);
143+
}
144+
C4x4 = -C4x4;
145+
DC4x4 = -DC4x4;
146+
//std::cout << DC4x4 << std::endl;
147+
148+
double factor = 1.0 / sqrt(1 - eigenvectors4d(3, min_eigenvalue_index) * eigenvectors4d(3, min_eigenvalue_index));
149+
if (eigenvectors4d(3, min_eigenvalue_index) < 0.0)
150+
factor = -factor;
151+
152+
normal(0) = factor * eigenvectors4d(0, min_eigenvalue_index);
153+
normal(1) = factor * eigenvectors4d(1, min_eigenvalue_index);
154+
normal(2) = factor * eigenvectors4d(2, min_eigenvalue_index);
155+
double bias = factor * eigenvectors4d(3, min_eigenvalue_index);
156+
//std::cout << it->normal.dot(normal) << " " << fabs(bias - it->bias) << std::endl;
157+
Eigen::Matrix3d Hnn_inv = Hnn.inverse ();
158+
Eigen::Matrix3d Hnn_prime = Hnn - (1/Hdd) * Hnd * Hnd.transpose();
159+
160+
eigensolver3d.compute(-Hnn_prime);
161+
eigenvalues3d = eigensolver3d.eigenvalues().real();
162+
eigenvectors3d = eigensolver3d.eigenvectors().real();
163+
min_eigenvalue = eigenvalues3d.cwiseAbs().minCoeff(&min_eigenvalue_index);
164+
Cnn = Eigen::Matrix3d::Zero ();
165+
for (int j = 0; j < 3; j++)
166+
{
167+
if (j == min_eigenvalue_index)
168+
continue;
169+
Cnn += eigenvectors3d.col(j) * eigenvectors3d.col(j).transpose() / eigenvalues3d(j);
170+
}
171+
172+
Cdd = -normal.dot(Hnn_inv * normal) / (normal.dot(Hnn_inv * Hnd) * normal.dot(Hnn_inv * Hnd));
173+
/* write the planar patch corresponding attributes. */
174+
d = bias;
175+
176+
scatter_matrix = S;
177+
area = segment->area;
178+
point_num = segment->point_num;
179+
}
180+
}

0 commit comments

Comments
 (0)