2012-09-16 12:33:11 +00:00
/*
* panorama implementation
*
* Copyright ( C ) HamidReza Houshiar
*
* Released under the GPL version 3.
*
*/
# include "slam6d/fbr/panorama.h"
using namespace std ;
namespace fbr {
void panorama : : init ( unsigned int width , unsigned int height , projection_method method , unsigned int numberOfImages , double param , panorama_map_method mMethod ) {
iWidth = width ;
iHeight = height ;
pMethod = method ;
nImages = numberOfImages ;
pParam = param ;
if ( mMethod = = FARTHEST ) {
iMap . create ( iHeight , iWidth , CV_32FC ( 3 ) ) ;
iMap = cv : : Scalar : : all ( 0 ) ;
}
else if ( mMethod = = EXTENDED ) {
extendedIMap . resize ( iHeight ) ;
for ( unsigned int i = 0 ; i < iHeight ; i + + )
extendedIMap [ i ] . resize ( iWidth ) ;
}
iReflectance . create ( iHeight , iWidth , CV_8U ) ;
iReflectance = cv : : Scalar : : all ( 0 ) ;
iRange . create ( iHeight , iWidth , CV_32FC ( 1 ) ) ;
iRange = cv : : Scalar : : all ( 0 ) ;
mapMethod = mMethod ;
}
panorama : : panorama ( unsigned int width , unsigned int height , projection_method method , unsigned int numberOfImages , double param , panorama_map_method mMethod ) {
init ( width , height , method , numberOfImages , param , mMethod ) ;
}
panorama : : panorama ( unsigned int width , unsigned int height , projection_method method , unsigned int numberOfImages , double param ) {
init ( width , height , method , numberOfImages , param , FARTHEST ) ;
}
panorama : : panorama ( unsigned int width , unsigned int height , projection_method method , unsigned int numberOfImages ) {
double param = 0 ;
if ( method = = PANNINI )
param = 1 ;
else if ( method = = STEREOGRAPHIC )
param = 2 ;
init ( width , height , method , numberOfImages , param , FARTHEST ) ;
}
panorama : : panorama ( unsigned int width , unsigned int height , projection_method method ) {
double param = 0 ;
unsigned int numberOfImages = 1 ;
if ( method = = RECTILINEAR )
numberOfImages = 3 ;
else if ( method = = PANNINI ) {
numberOfImages = 3 ;
param = 1 ;
} else if ( method = = STEREOGRAPHIC ) {
numberOfImages = 3 ;
param = 2 ;
}
init ( width , height , method , numberOfImages , param , FARTHEST ) ;
}
void panorama : : map ( int x , int y , cv : : MatIterator_ < cv : : Vec4f > it , double range ) {
iReflectance . at < uchar > ( y , x ) = ( * it ) [ 3 ] * 255 ; //reflectance
iRange . at < float > ( y , x ) = range ; //range
if ( mapMethod = = FARTHEST ) {
//adding the point with max distance
if ( iRange . at < float > ( y , x ) < range ) {
2012-10-02 07:37:52 +00:00
iMap . at < cv : : Vec3f > ( y , x ) [ 0 ] = ( * it ) [ 0 ] ; //x
iMap . at < cv : : Vec3f > ( y , x ) [ 1 ] = ( * it ) [ 1 ] ; //y
iMap . at < cv : : Vec3f > ( y , x ) [ 2 ] = ( * it ) [ 2 ] ; //z
2012-09-16 12:33:11 +00:00
}
} else if ( mapMethod = = EXTENDED ) {
//adding all the points
cv : : Vec3f point ;
point [ 0 ] = ( * it ) [ 0 ] ; //x
point [ 1 ] = ( * it ) [ 1 ] ; //y
point [ 2 ] = ( * it ) [ 2 ] ; //z
extendedIMap [ y ] [ x ] . push_back ( point ) ;
}
}
void panorama : : createPanorama ( cv : : Mat scan ) {
//EQUIRECTANGULAR projection
if ( pMethod = = EQUIRECTANGULAR ) {
//adding the longitude to x axis and latitude to y axis
double xFactor = ( double ) iWidth / 2 / M_PI ;
int widthMax = iWidth - 1 ;
double yFactor = ( double ) iHeight / ( ( MAX_ANGLE - MIN_ANGLE ) / 360 * 2 * M_PI ) ;
//shift all the valuse to positive points on image
double heightLow = ( 0 - MIN_ANGLE ) / 360 * 2 * M_PI ;
int heightMax = iHeight - 1 ;
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
int x = ( int ) ( xFactor * phi ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
int y = ( int ) ( yFactor * ( theta + heightLow ) ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
2012-10-02 07:37:52 +00:00
//CONIC projection
if ( pMethod = = CONIC ) {
// set up maximum latitude and longitude angles of the robot
double MIN_VERT_ANGLE = MIN_ANGLE * M_PI / 180.0 , MAX_VERT_ANGLE = MAX_ANGLE * M_PI / 180.0 ,
MIN_HORIZ_ANGLE = - M_PI , MAX_HORIZ_ANGLE = M_PI ;
// set up initial parameters according to MathWorld: http://mathworld.wolfram.com/AlbersEqual-AreaConicProjection.html
double Lat0 = 0. , Long0 = 0. ;
double Phi1 = - 40. * M_PI / 180.0 , Phi2 = 60 * M_PI / 180.0 ;
double n = ( sin ( Phi1 ) + sin ( Phi2 ) ) / 2. ;
double C = sqr ( cos ( Phi1 ) ) + 2 * n * sin ( Phi1 ) ;
double Rho0 = sqrt ( C - 2 * n * sin ( Lat0 ) ) / n ;
// set up max values for x and y and add the longitude to x axis and latitude to y axis
double xmax = ( 1. / n * sqrt ( C - 2 * n * sin ( MIN_VERT_ANGLE ) ) ) * sin ( n * ( MAX_HORIZ_ANGLE - Long0 ) ) ;
double xmin = ( 1. / n * sqrt ( C - 2 * n * sin ( MIN_VERT_ANGLE ) ) ) * sin ( n * ( MIN_HORIZ_ANGLE - Long0 ) ) ;
double xFactor = ( double ) iWidth / ( xmax - xmin ) ;
int widthMax = iWidth - 1 ;
double ymin = Rho0 - ( 1. / n * sqrt ( C - 2 * n * sin ( MIN_VERT_ANGLE ) ) ) * cos ( n * ( 0. - Long0 ) ) ;
double ymax = Rho0 - ( 1. / n * sqrt ( C - 2 * n * sin ( MAX_VERT_ANGLE ) ) ) * cos ( n * ( MAX_HORIZ_ANGLE - Long0 ) ) ;
double yFactor = ( double ) iHeight / ( ymax - ymin ) ;
//shift all the values to positive points on image
int heightMax = iHeight - 1 ;
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//phi == longitude == horizantal angle of view of [0:360]
phi = 180.0 - phi ;
phi * = M_PI / 180.0 ;
//theta == latitude == vertical angle of view of [-40:60]
theta - = 90 ;
theta * = - 1 ;
theta * = M_PI / 180.0 ;
// add minimum x position as an offset
int x = ( int ) ( xFactor * ( sqrt ( C - 2 * n * sin ( theta ) ) / n * sin ( n * ( phi - Long0 ) ) + fabs ( xmin ) ) ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
// add minimum y position as an offset
int y = ( int ) ( yFactor * ( Rho0 - ( 1 / n * sqrt ( C - 2 * n * sin ( theta ) ) ) * cos ( n * ( phi - Long0 ) ) + fabs ( ymin ) ) ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
2012-09-16 12:33:11 +00:00
//CYLINDRICAL projection
if ( pMethod = = CYLINDRICAL ) {
//adding the longitude to x and tan(latitude) to y
//find the x and y range
double xFactor = ( double ) iWidth / 2 / M_PI ;
int widthMax = iWidth - 1 ;
double yFactor = ( double ) iHeight / ( tan ( MAX_ANGLE / 360 * 2 * M_PI ) - tan ( MIN_ANGLE / 360 * 2 * M_PI ) ) ;
double heightLow = ( MIN_ANGLE ) / 360 * 2 * M_PI ;
int heightMax = iHeight - 1 ;
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
int x = ( int ) ( xFactor * phi ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
int y = ( int ) ( ( double ) yFactor * ( tan ( theta ) - tan ( heightLow ) ) ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
//Mercator Projection
if ( pMethod = = MERCATOR ) {
//find the x and y range
double xFactor = ( double ) iWidth / 2 / M_PI ;
int widthMax = iWidth - 1 ;
double yFactor = ( double ) iHeight / ( log ( tan ( MAX_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos ( MAX_ANGLE / 360 * 2 * M_PI ) ) ) - log ( tan ( MIN_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos ( MIN_ANGLE / 360 * 2 * M_PI ) ) ) ) ;
double heightLow = log ( tan ( MIN_ANGLE / 360 * 2 * M_PI ) + ( 1 / cos ( MIN_ANGLE / 360 * 2 * M_PI ) ) ) ;
int heightMax = iHeight - 1 ;
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
int x = ( int ) ( xFactor * phi ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
int y = ( int ) ( yFactor * ( log ( tan ( theta ) + ( 1 / cos ( theta ) ) ) - heightLow ) ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
//RECTILINEAR projection
if ( pMethod = = RECTILINEAR ) {
//default value for nImages
if ( nImages = = 0 ) nImages = 3 ;
cout < < " Number of images per scan is: " < < nImages < < endl ;
double l0 , p1 , iMinx , iMaxx , iMiny , iMaxy , interval ;
interval = 2 * M_PI / nImages ;
iMiny = - M_PI / 9 ;
iMaxy = 2 * M_PI / 9 ;
//latitude of projection center
p1 = 0 ;
//go through all points
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
for ( unsigned int j = 0 ; j < nImages ; j + + ) {
iMinx = j * interval ;
iMaxx = ( j + 1 ) * interval ;
//check for point in interval
if ( phi < iMaxx & & phi > iMinx ) {
double max , min , coscRectilinear ;
//the longitude of projection center
l0 = iMinx + interval / 2 ;
//finding the min and max of the x direction
coscRectilinear = sin ( p1 ) * sin ( iMaxy ) + cos ( p1 ) * cos ( iMaxy ) * cos ( iMaxx - l0 ) ;
max = ( cos ( iMaxy ) * sin ( iMaxx - l0 ) / coscRectilinear ) ;
coscRectilinear = sin ( p1 ) * sin ( iMiny ) + cos ( p1 ) * cos ( iMiny ) * cos ( iMinx - l0 ) ;
min = ( cos ( iMiny ) * sin ( iMinx - l0 ) / coscRectilinear ) ;
double xFactor = ( double ) ( iWidth / nImages ) / ( max - min ) ;
double xlow = min ;
int widthMax = ( iWidth / nImages ) - 1 ;
//finding the min and max of y direction
coscRectilinear = sin ( p1 ) * sin ( iMaxy ) + cos ( p1 ) * cos ( iMaxy ) * cos ( iMaxx - l0 ) ;
max = ( ( cos ( p1 ) * sin ( iMaxy ) - sin ( p1 ) * cos ( iMaxy ) * cos ( iMaxx - l0 ) ) / coscRectilinear ) ;
coscRectilinear = sin ( p1 ) * sin ( iMiny ) + cos ( p1 ) * cos ( iMiny ) * cos ( iMinx - l0 ) ;
min = ( ( cos ( p1 ) * sin ( iMiny ) - sin ( p1 ) * cos ( iMiny ) * cos ( iMinx - l0 ) ) / coscRectilinear ) ;
double yFactor = ( double ) iHeight / ( max - min ) ;
double heightLow = min ;
int heightMax = iHeight - 1 ;
//project the points and add them to image
coscRectilinear = sin ( p1 ) * sin ( theta ) + cos ( p1 ) * cos ( theta ) * cos ( phi - l0 ) ;
int x = ( int ) ( xFactor ) * ( ( cos ( theta ) * sin ( phi - l0 ) / coscRectilinear ) - xlow ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
x = x + ( j * iWidth / nImages ) ;
int y = ( int ) ( yFactor ) * ( ( ( cos ( p1 ) * sin ( theta ) - sin ( p1 ) * cos ( theta ) * cos ( phi - l0 ) ) / coscRectilinear ) - heightLow ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
}
}
//PANNINI projection
if ( pMethod = = PANNINI ) {
//default values for nImages and dPannini==pParam
if ( pParam = = 0 ) pParam = 1 ;
if ( nImages = = 0 ) nImages = 3 ;
cout < < " Parameter d is: " < < pParam < < " , Number of images per scan is: " < < nImages < < endl ;
double l0 , p1 , iMinx , iMaxx , iMiny , iMaxy , interval ;
interval = 2 * M_PI / nImages ;
iMiny = - M_PI / 9 ;
iMaxy = 2 * M_PI / 9 ;
//latitude of projection center
p1 = 0 ;
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
for ( unsigned int j = 0 ; j < nImages ; j + + ) {
iMinx = j * interval ;
iMaxx = ( j + 1 ) * interval ;
//check for point in interval
if ( phi < ( iMaxx ) & & phi > ( iMinx ) ) {
double max , min , sPannini ;
//the longitude of projection center
l0 = iMinx + interval / 2 ;
//use the S variable of pannini projection mentioned in the thesis
//finding the min and max of the x direction
sPannini = ( pParam + 1 ) / ( pParam + sin ( p1 ) * tan ( iMaxy ) + cos ( p1 ) * cos ( iMaxx - l0 ) ) ;
max = sPannini * ( sin ( iMaxx - l0 ) ) ;
sPannini = ( pParam + 1 ) / ( pParam + sin ( p1 ) * tan ( iMiny ) + cos ( p1 ) * cos ( iMinx - l0 ) ) ;
min = sPannini * ( sin ( iMinx - l0 ) ) ;
double xFactor = ( double ) ( iWidth / nImages ) / ( max - min ) ;
double xlow = min ;
int widthMax = ( iWidth / nImages ) - 1 ;
//finding the min and max of y direction
sPannini = ( pParam + 1 ) / ( pParam + sin ( p1 ) * tan ( iMaxy ) + cos ( p1 ) * cos ( iMaxx - l0 ) ) ;
max = sPannini * ( tan ( iMaxy ) * ( cos ( p1 ) - sin ( p1 ) * 1 / tan ( iMaxy ) * cos ( iMaxx - l0 ) ) ) ;
sPannini = ( pParam + 1 ) / ( pParam + sin ( p1 ) * tan ( iMiny ) + cos ( p1 ) * cos ( iMinx - l0 ) ) ;
min = sPannini * ( tan ( iMiny ) * ( cos ( p1 ) - sin ( p1 ) * 1 / tan ( iMiny ) * cos ( iMinx - l0 ) ) ) ;
double yFactor = ( double ) iHeight / ( max - min ) ;
double heightLow = min ;
int heightMax = iHeight - 1 ;
//project the points and add them to image
sPannini = ( pParam + 1 ) / ( pParam + sin ( p1 ) * tan ( theta ) + cos ( p1 ) * cos ( phi - l0 ) ) ;
int x = ( int ) ( xFactor ) * ( sPannini * sin ( phi - l0 ) - xlow ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
x = x + ( j * iWidth / nImages ) ;
int y = ( int ) ( yFactor ) * ( ( sPannini * tan ( theta ) * ( cos ( p1 ) - sin ( p1 ) * ( 1 / tan ( theta ) ) * cos ( phi - l0 ) ) ) - heightLow ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
}
}
//STEREOGRAPHIC projection
if ( pMethod = = STEREOGRAPHIC ) {
//default values for nImages and rStereographic==pParam
if ( pParam = = 0 ) pParam = 2 ;
if ( nImages = = 0 ) nImages = 3 ;
cout < < " Paremeter R is: " < < pParam < < " , Number of images per scan is: " < < nImages < < endl ;
// l0 and p1 are the center of projection iminx, imaxx, iminy, imaxy are the bounderis of intervals
double l0 , p1 , iMinx , iMaxx , iMiny , iMaxy , interval ;
interval = 2 * M_PI / nImages ;
iMiny = - M_PI / 9 ;
iMaxy = 2 * M_PI / 9 ;
//latitude of projection center
p1 = 0 ;
//go through all points
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
for ( unsigned int j = 0 ; j < nImages ; j + + ) {
iMinx = j * interval ;
iMaxx = ( j + 1 ) * interval ;
//check for point in intervals
if ( phi < ( iMaxx ) & & phi > ( iMinx ) ) {
double max , min , k ;
//longitude of projection center
l0 = iMinx + interval / 2 ;
//use the R variable of stereographic projection mentioned in the thesis
//finding the min and max of x direction
k = ( 2 * pParam ) / ( 1 + sin ( p1 ) * sin ( p1 ) + cos ( p1 ) * cos ( p1 ) * cos ( iMaxx - l0 ) ) ;
max = k * cos ( p1 ) * sin ( iMaxx - l0 ) ;
k = ( 2 * pParam ) / ( 1 + sin ( p1 ) * sin ( p1 ) + cos ( p1 ) * cos ( p1 ) * cos ( iMinx - l0 ) ) ;
min = k * cos ( p1 ) * sin ( iMinx - l0 ) ;
double xFactor = ( double ) ( iWidth / nImages ) / ( max - min ) ;
double xlow = min ;
int widthMax = ( iWidth / nImages ) - 1 ;
//finding the min and max of y direction
k = ( 2 * pParam ) / ( 1 + sin ( p1 ) * sin ( iMaxy ) + cos ( p1 ) * cos ( iMaxy ) * cos ( iMaxx - l0 ) ) ;
max = k * ( cos ( p1 ) * sin ( iMaxy ) - sin ( p1 ) * cos ( iMaxy ) * cos ( iMaxx - l0 ) ) ;
k = ( 2 * pParam ) / ( 1 + sin ( p1 ) * sin ( iMiny ) + cos ( p1 ) * cos ( iMiny ) * cos ( iMinx - l0 ) ) ;
min = k * ( cos ( p1 ) * sin ( iMiny ) - sin ( p1 ) * cos ( iMiny ) * cos ( iMinx - l0 ) ) ;
double yFactor = ( double ) iHeight / ( max - min ) ;
double heightLow = min ;
int heightMax = iHeight - 1 ;
//project the points and add them to image
k = ( 2 * pParam ) / ( 1 + sin ( p1 ) * sin ( theta ) + cos ( p1 ) * cos ( theta ) * cos ( phi - l0 ) ) ;
int x = ( int ) ( xFactor ) * ( k * cos ( theta ) * sin ( phi - l0 ) - xlow ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
x = x + ( j * iWidth / nImages ) ;
int y = ( int ) ( yFactor ) * ( k * ( cos ( p1 ) * sin ( theta ) - sin ( p1 ) * cos ( theta ) * cos ( phi - l0 ) ) - heightLow ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
}
}
//ZAXIS projection
if ( pMethod = = ZAXIS ) {
double zmin = - 200 ;
double zmax = 4000 ;
//adding the longitude to x axis and latitude to y axis
double xFactor = ( double ) iWidth / 2 / M_PI ;
int widthMax = iWidth - 1 ;
cout < < " ZMAX= " < < zmax < < " ZMIN= " < < zmin < < endl ;
double yFactor = ( double ) iHeight / ( zmax - zmin ) ;
//shift all the valuse to positive points on image
double heightLow = zmin ;
int heightMax = iHeight - 1 ;
cv : : MatIterator_ < cv : : Vec4f > it , end ;
for ( it = scan . begin < cv : : Vec4f > ( ) , end = scan . end < cv : : Vec4f > ( ) ; it ! = end ; + + it ) {
double kart [ 3 ] , polar [ 3 ] , phi , theta , range ;
kart [ 0 ] = ( * it ) [ 2 ] / 100 ;
kart [ 1 ] = ( * it ) [ 0 ] / - 100 ;
kart [ 2 ] = ( * it ) [ 1 ] / 100 ;
toPolar ( kart , polar ) ;
//theta == polar[0] == scan [4]
//phi == polar[1] == scan [5]
//range == polar[2] == scan [3]
theta = polar [ 0 ] * 180 / M_PI ;
phi = polar [ 1 ] * 180 / M_PI ;
range = polar [ 2 ] ;
//horizantal angle of view of [0:360] and vertical of [-40:60]
phi = 360.0 - phi ;
phi = phi * 2.0 * M_PI / 360.0 ;
theta - = 90 ;
theta * = - 1 ;
theta * = 2.0 * M_PI / 360.0 ;
int x = ( int ) ( xFactor * phi ) ;
if ( x < 0 ) x = 0 ;
if ( x > widthMax ) x = widthMax ;
///////////////////check this
int y = ( int ) ( yFactor * ( ( * it ) [ 1 ] - heightLow ) ) ;
y = heightMax - y ;
if ( y < 0 ) y = 0 ;
if ( y > heightMax ) y = heightMax ;
//create the iReflectance iRange and map
map ( x , y , it , range ) ;
}
}
}
unsigned int panorama : : getImageWidth ( ) {
return iWidth ;
}
unsigned int panorama : : getImageHeight ( ) {
return iHeight ;
}
projection_method panorama : : getProjectionMethod ( ) {
return pMethod ;
}
unsigned int panorama : : getNumberOfImages ( ) {
return nImages ;
}
double panorama : : getProjectionParam ( ) {
return pParam ;
}
cv : : Mat panorama : : getReflectanceImage ( ) {
return iReflectance ;
}
cv : : Mat panorama : : getMap ( ) {
return iMap ;
}
cv : : Mat panorama : : getRangeImage ( ) {
return iRange ;
}
vector < vector < vector < cv : : Vec3f > > > panorama : : getExtendedMap ( ) {
return extendedIMap ;
}
panorama_map_method panorama : : getMapMethod ( ) {
return mapMethod ;
}
void panorama : : getDescription ( ) {
cout < < " panorama created with width: " < < iWidth < < " , and height: " < < iHeight < < " , and projection method: " < < projectionMethodToString ( pMethod ) < < " , number of images: " < < nImages < < " , projection param: " < < pParam < < " . " < < endl ;
cout < < endl ;
}
}