302 lines
8.8 KiB
Text
302 lines
8.8 KiB
Text
/**
|
|
* Representation of the Accumulator Cube.
|
|
*
|
|
* This file contains the functions necessary to perform the transformations
|
|
* between normal vectors and cube coordinates. The representation is taken
|
|
* from
|
|
* A. Censi and S. Carpin,
|
|
* HSM3D: Feature-Less Global 6DOF Scan-Matching in the Hough/Radon Domain,
|
|
* In Proceedings of the IEEE International Conference on Robotics and
|
|
* Automation, 2009.
|
|
*
|
|
* The code in this file is a modified version of the code given by the authors
|
|
* on http://purl.org/censi/2008/hsm3d
|
|
*
|
|
* We gratefully appreciate the work of the authors and thank them for making
|
|
* the code publically available.
|
|
*
|
|
* @author Dorit Borrmann. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
/*
|
|
|
|
AUTORIGHTS
|
|
Copyright (c) 2008 The Regents of the University of California.
|
|
All Rights Reserved.
|
|
|
|
Created by Stefano Carpin
|
|
University of California, Merced, Robotics Lab - School of Engineering
|
|
|
|
Permission to use, copy, modify, and distribute this software and its
|
|
documentation for educational, research and non-profit purposes, without fee,
|
|
and without a written agreement is hereby granted, provided that the above
|
|
copyright notice, this paragraph and the following three paragraphs appear in
|
|
all copies.
|
|
|
|
This software program and documentation are copyrighted by The Regents of the
|
|
University of California. The software program and documentation are supplied
|
|
"as is", without any accompanying services from The Regents. The Regents does
|
|
not warrant that the operation of the program will be uninterrupted or
|
|
error-free. The end-user understands that the program was developed for
|
|
research purposes and is advised not to rely exclusively on the program for
|
|
any reason.
|
|
|
|
IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
|
|
DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
|
|
LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION,
|
|
EVEN IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
|
SUCH DAMAGE. THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY
|
|
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
|
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED
|
|
HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO
|
|
OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
|
|
MODIFICATIONS.
|
|
|
|
*/
|
|
#ifndef __HSM3D_ICC__
|
|
#define __HSM3D_ICC__
|
|
|
|
#include "shapes/accumulator.h"
|
|
|
|
bool real_compare(double x, double y) {
|
|
return fabs(x - y) < 0.000000001;
|
|
}
|
|
|
|
buffer_point AccumulatorCube::coords_s2_to_cell(double* s, unsigned int width) {
|
|
|
|
cube_point cubi;
|
|
|
|
double s_norm =sqrt(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
|
|
s[0] = s[0] / s_norm;
|
|
s[1] = s[1] / s_norm;
|
|
s[2] = s[2] / s_norm;
|
|
|
|
double sa[3];
|
|
sa[0] = fabs(s[0]);
|
|
sa[1] = fabs(s[1]);
|
|
sa[2] = fabs(s[2]);
|
|
|
|
if ( ( sa[0] >= sa[1] ) && ( sa[0] >= sa[2] ) ) {
|
|
cubi.face = s[0] > 0 ? 1 : 4;
|
|
cubi.u = s[1] / s[0];
|
|
cubi.v = s[2] / s[0];
|
|
}
|
|
|
|
else if ( ( sa[1] >= sa[0] ) && ( sa[1] >= sa[2] ) ) {
|
|
cubi.face = s[1] > 0 ? 2 : 5;
|
|
cubi.u = s[0] / s[1];
|
|
cubi.v = s[2] / s[1];
|
|
}
|
|
|
|
else if ( ( sa[2] >= sa[1] ) && ( sa[2] >= sa[0] ) ) {
|
|
cubi.face = s[2] > 0 ? 3 : 6;
|
|
cubi.u = s[0] / s[2];
|
|
cubi.v = s[1] / s[2];
|
|
} else {
|
|
cubi.face = 0;
|
|
cubi.u = 0;
|
|
cubi.v = 0;
|
|
}
|
|
|
|
buffer_point retval;
|
|
|
|
double u = (cubi.u + 1)/2;
|
|
double v = (cubi.v + 1)/2;
|
|
|
|
unsigned int n = width;
|
|
|
|
retval.face = cubi.face;
|
|
|
|
retval.i = real_compare(u,1) ? n : 1 + floor(u*n);
|
|
retval.j = real_compare(v,1) ? n : 1 + floor(v*n);
|
|
|
|
return retval;
|
|
|
|
}
|
|
|
|
double* AccumulatorCube::coords_cube_to_s2(buffer_point src, unsigned int width) {
|
|
|
|
cube_point retval;
|
|
retval.face = src.face;
|
|
retval.u = (src.i - 0.5 ) / (static_cast<double>(width));
|
|
retval.u = retval.u*2 - 1;
|
|
|
|
retval.v = (src.j - 0.5 ) / (static_cast<double>(width));
|
|
retval.v = retval.v*2 - 1;
|
|
|
|
double *result = new double[3];
|
|
|
|
switch ( retval.face ) {
|
|
case 1:
|
|
result[0] = 1;
|
|
result[1] = retval.u;
|
|
result[2] = retval.v;
|
|
break;
|
|
case 4:
|
|
result[0] = -1;
|
|
result[1] = -retval.u;
|
|
result[2] = -retval.v;
|
|
break;
|
|
case 2:
|
|
result[0] = retval.u;
|
|
result[1] = 1;
|
|
result[2] = retval.v;
|
|
break;
|
|
case 5:
|
|
result[0] = -retval.u;
|
|
result[1] = -1;
|
|
result[2] = -retval.v;
|
|
break;
|
|
case 3:
|
|
result[0] = retval.u;
|
|
result[1] = retval.v;
|
|
result[2] = 1;
|
|
break;
|
|
case 6:
|
|
result[0] = -retval.u;
|
|
result[1] = -retval.v;
|
|
result[2] = -1;
|
|
break;
|
|
}
|
|
|
|
double norm = sqrt(result[0]*result[0] + result[1]*result[1] + result[2]*result[2]);
|
|
result[0] /= norm;
|
|
result[1] /= norm;
|
|
result[2] /= norm;
|
|
|
|
return result;
|
|
}
|
|
|
|
void AccumulatorCube::coords_cube_for_print(buffer_point src, double** result, unsigned int width) {
|
|
/*
|
|
assert((src.face >=1 ) && (src.face <=6 ));
|
|
assert((src.i >= 1 ) && ( src.i <= width));
|
|
assert((src.j >= 1 ) && ( src.j <= width));
|
|
*/
|
|
|
|
cube_point retval1;
|
|
cube_point retval2;
|
|
cube_point retval3;
|
|
cube_point retval4;
|
|
retval1.face = src.face;
|
|
retval2.face = src.face;
|
|
retval3.face = src.face;
|
|
retval4.face = src.face;
|
|
|
|
retval1.u = (src.i - 1.0 ) / (static_cast<double>(width));
|
|
retval2.u = (src.i - 1.0 ) / (static_cast<double>(width));
|
|
retval1.u = retval1.u*2.0 - 1.0;
|
|
retval2.u = retval2.u*2.0 - 1.0;
|
|
|
|
retval1.v = (src.j - 1.0 ) / (static_cast<double>(width));
|
|
retval2.v = (src.j) / (static_cast<double>(width));
|
|
retval1.v = retval1.v*2 - 1;
|
|
retval2.v = retval2.v*2 - 1;
|
|
|
|
retval3.u = (src.i) / (static_cast<double>(width));
|
|
retval4.u = (src.i) / (static_cast<double>(width));
|
|
retval3.u = retval3.u*2.0 - 1.0;
|
|
retval4.u = retval4.u*2.0 - 1.0;
|
|
|
|
retval3.v = (src.j) / (static_cast<double>(width));
|
|
retval4.v = (src.j - 1.0) / (static_cast<double>(width));
|
|
retval3.v = retval3.v*2 - 1;
|
|
retval4.v = retval4.v*2 - 1;
|
|
/*
|
|
assert((src.face >=1 ) && (src.face <=6 ));
|
|
assert((src.u >= -1 ) && ( src.u <= 1));
|
|
assert((src.v >= -1 ) && ( src.v <= 1));
|
|
*/
|
|
|
|
// double *result = new double[2][3];
|
|
|
|
switch ( retval1.face ) {
|
|
case 1:
|
|
result[0][0] = 1;
|
|
result[0][1] = retval1.u;
|
|
result[0][2] = retval1.v;
|
|
result[1][0] = 1;
|
|
result[1][1] = retval2.u;
|
|
result[1][2] = retval2.v;
|
|
result[2][0] = 1;
|
|
result[2][1] = retval3.u;
|
|
result[2][2] = retval3.v;
|
|
result[3][0] = 1;
|
|
result[3][1] = retval4.u;
|
|
result[3][2] = retval4.v;
|
|
break;
|
|
case 4:
|
|
result[0][0] = -1;
|
|
result[0][1] = -retval1.u;
|
|
result[0][2] = -retval1.v;
|
|
result[1][0] = -1;
|
|
result[1][1] = -retval2.u;
|
|
result[1][2] = -retval2.v;
|
|
result[2][0] = -1;
|
|
result[2][1] = -retval3.u;
|
|
result[2][2] = -retval3.v;
|
|
result[3][0] = -1;
|
|
result[3][1] = -retval4.u;
|
|
result[3][2] = -retval4.v;
|
|
break;
|
|
case 2:
|
|
result[0][0] = retval1.u;
|
|
result[0][1] = 1;
|
|
result[0][2] = retval1.v;
|
|
result[1][0] = retval2.u;
|
|
result[1][1] = 1;
|
|
result[1][2] = retval2.v;
|
|
result[2][0] = retval3.u;
|
|
result[2][1] = 1;
|
|
result[2][2] = retval3.v;
|
|
result[3][0] = retval4.u;
|
|
result[3][1] = 1;
|
|
result[3][2] = retval4.v;
|
|
break;
|
|
case 5:
|
|
result[0][0] = -retval1.u;
|
|
result[0][1] = -1;
|
|
result[0][2] = -retval1.v;
|
|
result[1][0] = -retval2.u;
|
|
result[1][1] = -1;
|
|
result[1][2] = -retval2.v;
|
|
result[2][0] = -retval3.u;
|
|
result[2][1] = -1;
|
|
result[2][2] = -retval3.v;
|
|
result[3][0] = -retval4.u;
|
|
result[3][1] = -1;
|
|
result[3][2] = -retval4.v;
|
|
break;
|
|
case 3:
|
|
result[0][0] = retval1.u;
|
|
result[0][1] = retval1.v;
|
|
result[0][2] = 1;
|
|
result[1][0] = retval2.u;
|
|
result[1][1] = retval2.v;
|
|
result[1][2] = 1;
|
|
result[2][0] = retval3.u;
|
|
result[2][1] = retval3.v;
|
|
result[2][2] = 1;
|
|
result[3][0] = retval4.u;
|
|
result[3][1] = retval4.v;
|
|
result[3][2] = 1;
|
|
break;
|
|
case 6:
|
|
result[0][0] = -retval1.u;
|
|
result[0][1] = -retval1.v;
|
|
result[0][2] = -1;
|
|
result[1][0] = -retval2.u;
|
|
result[1][1] = -retval2.v;
|
|
result[1][2] = -1;
|
|
result[2][0] = -retval3.u;
|
|
result[2][1] = -retval3.v;
|
|
result[2][2] = -1;
|
|
result[3][0] = -retval4.u;
|
|
result[3][1] = -retval4.v;
|
|
result[3][2] = -1;
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|