void furnace::furnaceTop::drawSelf() {
debug("furnaceTop::draw()");
colorV(blue);
//front
//The front face is a concave polygon, so it has to be drawn in two halves
object::drawVertex(frontFace,6,GL_POLYGON);
glPushMatrix();
glRotatef(180,0,1,0);
object::drawVertex(frontFace,6,GL_POLYGON);
glPopMatrix();
//sides
colorV(red);
object::drawVertex(sides,8,GL_QUAD_STRIP);
object::drawVertex(top, 4, GL_POLYGON);
}
/*
region<int> getDiffer( trackDocument * doc, int areaIndex, int start, int end, int time )
{
boundaryArea & area = doc->areas[areaIndex];
boundaryCurve & curve = area.divisions.first();
boundaryCurveIndex index;
index.curve = 0;
index.size = curve.points.size;
index.start = start;
index.end = end;
static array2<bool> flag;
flag.allocate( doc->width, doc->height );
region<int> result;
if ( ! time ) return result;
area.update( time - 1 );
area.update( time );
region<int> prevShape, nowShape;
{
list< point2<int> > segments;
bool checkStart = true;
for ( boundaryCurveIndex::iterator iti( index ); iti; ++iti ) {
curve.segment( segments, iti(), time - 1 );
}
memset( flag.data, 0, sizeof( bool ) * flag.size );
for ( list< point2<int> >::iterator itp( segments ); itp; ++itp ) {
flag( clamp( 0, itp->x, doc->width - 1 ), clamp( 0, itp->y, doc->height - 1 ) ) = true;
}
prevShape.set( flag, 0, 0 );
}
{
list< point2<int> > segments;
bool checkStart = true;
for ( boundaryCurveIndex::iterator iti( index ); iti; ++iti ) {
curve.segment( segments, iti(), time );
}
memset( flag.data, 0, sizeof( bool ) * flag.size );
for ( list< point2<int> >::iterator itp( segments ); itp; ++itp ) {
flag( clamp( 0, itp->x, doc->width - 1 ), clamp( 0, itp->y, doc->height - 1 ) ) = true;
}
nowShape.set( flag, 0, 0 );
}
region<int> round;
round = nowShape | prevShape;
{
region<int> line;
point2<int> p1 = curve.points[start]( time - 1 );
point2<int> p2 = curve.points[start]( time );
line.line( p1.x, p1.y, p2.x, p2.y );
round |= line;
}
{
region<int> line;
point2<int> p1 = curve.points[end]( time - 1 );
point2<int> p2 = curve.points[end]( time );
line.line( p1.x, p1.y, p2.x, p2.y );
round |= line;
}
round.fill( result );
return result;
}
*/
void stateDocument::paint()
{
trackDocument * doc = trackDocument::get();
trackView * view = trackView::get();
if ( ! doc ) return;
if ( ! view ) return;
static image viewImage;//画面表示用画像
viewImage.topdown = false;
double maxValue = doc->maxValues[doc->currentViewImageIndex];
double windowLevel = windowLevelBar->get();
double windowSize = windowSizeBar->get();
decimal rate = 255.0 / windowSize;
decimal offset = windowLevel - windowSize / 2;
imageInterface< pixelLuminance<int16> > * img = & doc->originalImages[doc->currentViewImageIndex];
switch ( view->mode & trackView::baseImageMask ) {
case trackView::original:
break;
case trackView::vertEdge:
img = & doc->verticalEdgeImages[doc->currentViewImageIndex];
break;
case trackView::horzEdge:
img = & doc->horizontalEdgeImages[doc->currentViewImageIndex];
break;
default:
offset = 0;
rate = 0;
break;
}
viewImage.create( img->width, img->height );
pixel p( 0, 0, 0, 255 );
if ( rate ) {
for ( int y = 0; y < img->height; ++y ) {
for ( int x = 0; x < img->width; ++x ) {
p.r = p.g = p.b = static_cast<int8>( clamp<decimal>( 0,
( img->getInternal( x, y ).y - offset ) * rate, 255.0 ) );
viewImage.setInternal( x, y, p );
}
}
} else {
for ( int y = 0; y < img->height; ++y ) {
for ( int x = 0; x < img->width; ++x ) {
viewImage.setInternal( x, y, p );
}
}
}
//波の表示用の色
pixel colorV( 0, 0, 255, 255 );
pixel colorH( 0, 255, 0, 255 );
pixel colorA( 255, 255, 0, 255 );
pixel colorD( 255, 0, 255, 255 );
//論文投稿用の表示
if ( false && ( ( view->mode & trackView::baseImageMask ) == trackView::none ) ) {
p.r = p.g = p.b = 0;
for ( int y = 0; y < img->height; ++y ) {
for ( int x = 0; x < img->width; ++x ) {
viewImage.setInternal( x, y, p );
}
}
for ( int t = 0; t < doc->sizeTime(); ++t ) {
pixel c;
if ( view->mode & trackView::vertical ) {
for ( array< list<link> >::iterator ita( doc->verticalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
c.r = c.g = c.b = clamp<int>( 0, it->reliability * 255, 255 );
int x = it->cx * doc->wave[t] + it->bx;
int y = it->cy * doc->wave[t] + it->by;
viewImage.set( x, y, blendMax( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::horizontal ) {
for ( array< list<link> >::iterator ita( doc->horizontalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
c.r = c.g = c.b = clamp<int>( 0, it->reliability * 255, 255 );
int x = it->cx * doc->wave[t] + it->bx;
int y = it->cy * doc->wave[t] + it->by;
viewImage.set( x, y, blendMax( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::ascent ) {
for ( array< list<link> >::iterator ita( doc->ascentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
c.r = c.g = c.b = clamp<int>( 0, it->reliability * 255, 255 );
int x = it->cx * doc->wave[t] + it->bx;
int y = it->cy * doc->wave[t] + it->by;
viewImage.set( x, y, blendMax( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::descent ) {
for ( array< list<link> >::iterator ita( doc->descentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
c.r = c.g = c.b = clamp<int>( 0, it->reliability * 255, 255 );
int x = it->cx * doc->wave[t] + it->bx;
int y = it->cy * doc->wave[t] + it->by;
viewImage.set( x, y, blendMax( viewImage.get( x, y ), c ) );
}
}
}
}
} else
if ( view->mode != trackView::original ) {
/*
if ( view->mode & trackView::vertical ) {
checkMaximum<double> mx;
for ( array< list<link> >::iterator ita( doc->verticalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
mx( it->intensity );
}
}
for ( array< list<link> >::iterator ita( doc->verticalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorV;
c.a = clamp<int>( 0, c.a * it->intensity / mx(), 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::horizontal ) {
checkMaximum<double> mx;
for ( array< list<link> >::iterator ita( doc->horizontalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
mx( it->intensity );
}
}
for ( array< list<link> >::iterator ita( doc->horizontalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorH;
c.a = clamp<int>( 0, c.a * it->intensity / mx(), 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::ascent ) {
checkMaximum<double> mx;
for ( array< list<link> >::iterator ita( doc->ascentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
mx( it->intensity );
}
}
for ( array< list<link> >::iterator ita( doc->ascentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorA;
c.a = clamp<int>( 0, c.a * it->intensity / mx(), 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::descent ) {
checkMaximum<double> mx;
for ( array< list<link> >::iterator ita( doc->descentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
mx( it->intensity );
}
}
for ( array< list<link> >::iterator ita( doc->descentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorD;
c.a = clamp<int>( 0, c.a * it->intensity / mx(), 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
*/
if ( view->mode & trackView::vertical ) {
for ( array< list<link> >::iterator ita( doc->verticalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorV;
c.a = clamp<int>( 0, c.a * it->reliability, 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::horizontal ) {
for ( array< list<link> >::iterator ita( doc->horizontalLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorH;
c.a = clamp<int>( 0, c.a * it->reliability, 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::ascent ) {
for ( array< list<link> >::iterator ita( doc->ascentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorA;
c.a = clamp<int>( 0, c.a * it->reliability, 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
if ( view->mode & trackView::descent ) {
for ( array< list<link> >::iterator ita( doc->descentLinks ); ita; ++ita ) {
for ( list<link>::iterator it( ita() ); it; ++it ) {
pixel c = colorD;
c.a = clamp<int>( 0, c.a * it->reliability, 255 );
int x = it->cx * doc->wave[doc->currentViewImageIndex] + it->bx;
int y = it->cy * doc->wave[doc->currentViewImageIndex] + it->by;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
}
/*
for ( int i = 0; i < 4; ++i ) {
for ( array2<flicker>::iterator it( doc->flickers[i] ); it; ++it ) {
if ( it->value < 0.25 ) continue;
pixel c( 0, 255, 0, 255 );
c.a = clamp<int>( 0, c.a * it->value, 128 );
const point2<int> & pos = it->lnk.position( doc->currentViewImageIndex, doc->wave );
int x = pos.x, y = pos.y;
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
*/
for ( list< boundaryArea >::iterator it( doc->areas ); it; ++it ) {
it->update( doc->currentViewImageIndex );
}
list< point2<int> > points;
pixel linecolor( 255, 255, 0, 128 );
pixel pointcolor( 0, 255, 0, 255 );
pixel itnitialpointcolor( 0, 0, 255, 255 );
pixel editedpointcolor( 255, 0, 0, 255 );
/*
for ( list< boundaryArea >::iterator it( doc->areas ); it; ++it ) {
boundaryArea & area = it();
if ( ! area.enable ) continue;
pixel areacolor( 0, 0, 0, 64 );
for ( list< boundaryPart >::iterator ita( area.parts ); ita; ++ita ) {
areacolor.r = rand() % 256;
areacolor.g = rand() % 256;
areacolor.b = rand() % 256;
areacolor.a = 128;
boundaryPart & part = ita();
for ( region<int>::iterator itp( part.shapes[doc->currentViewImageIndex] ); itp; ++itp ) {
int x = itp->x;
int y = itp->y;
viewImage.set( x, y, blend( viewImage.get( x, y ), areacolor ) );
}
}
}
*/
for ( list< boundaryArea >::iterator it( doc->areas ); it; ++it ) {
//曲線の表示
if ( doc->show_curve )
{
for ( list< point2<int> >::iterator itp( it->boundaryCurves[doc->currentViewImageIndex] ); itp; ++itp ) {
const point2<int> & p = itp();
int x = p.x;
int y = p.y;
viewImage.set( x, y, blend( viewImage.get( x, y ), linecolor ) );
}
}
//制御点の表示
for ( list<boundaryPoint*>::iterator itc( it->controlPoints ); itc; ++itc ) {
boundaryPoint & bp = *itc();
const point2<int> & p = bp( doc->currentViewImageIndex );
pixel c;
switch ( bp.type( doc->currentViewImageIndex ) ) {
case boundaryPointFrame::typeInitial: c = itnitialpointcolor; break;
case boundaryPointFrame::typeEdited: c = editedpointcolor; break;
case boundaryPointFrame::typeInterpolated: c = pointcolor; break;
default: c = pointcolor; break;
}
for ( int y = p.y - 1; y <= p.y + 1; ++y ) {
for ( int x = p.x - 1; x <= p.x + 1; ++x ) {
viewImage.set( x, y, blend( viewImage.get( x, y ), c ) );
}
}
}
}
paint( viewImage );
}
int main (int argc, char** argv)
{
const Eigen::Vector3i resolution = Eigen::Vector3i(512, 512, 512);
//const Eigen::Vector3f volumeSize = Eigen::Vector3f(0.24f, 0.24, 0.24f);
const Eigen::Vector3f volumeSize = Eigen::Vector3f(4.0f, 4.0f, 4.0f);
int width = 512;
int height = 424;
float depthThres = 2.0f;
bool doVoxelFilter = false;
TsdfVolume tsdfV(resolution, volumeSize);
ColorVolume colorV(tsdfV);
EvoRec evoRec(tsdfV, colorV, width, height, depthThres, doVoxelFilter);
// begins process evoslam data
FrameInfo Kf1;
Kf1.rotMatUnscaled = Eigen::Matrix3f::Identity();
/*Kf1.rotMatUnscaled(0, 0) = 1;
Kf1.rotMatUnscaled(0, 1) = 0.0215;
Kf1.rotMatUnscaled(0, 2) = 0;
Kf1.rotMatUnscaled(1, 0) = 0;
Kf1.rotMatUnscaled(1, 1) = 0.9877;
Kf1.rotMatUnscaled(1, 2) = 0.1546;
Kf1.rotMatUnscaled(2, 0) = 0;
Kf1.rotMatUnscaled(2, 1) = -0.1546;
Kf1.rotMatUnscaled(2, 2) = 0.9877;
Kf1.transVec = Eigen::Vector3f(-0.05, -0.28, -0.93);*/
Kf1.transVec = Eigen::Vector3f(0, 0, 0);
//Kf1.transVec = Eigen::Vector3f(1.5, 1.5, 1.5);
Kf1.scale = 1;
Kf1.intr = Intr(367.221, 367.221, 252.952, 208.622);
FrameInfo Kf2;
Kf2.rotMatUnscaled = Eigen::Matrix3f::Identity();
Kf2.rotMatUnscaled(0, 0) = 0.134495157429655;
Kf2.rotMatUnscaled(0, 1) = 0.080498653293076;
Kf2.rotMatUnscaled(0, 2) = -0.987639113971279;
Kf2.rotMatUnscaled(1, 0) = -0.144135633421401;
Kf2.rotMatUnscaled(1, 1) = 0.987683785283996;
Kf2.rotMatUnscaled(1, 2) = 0.060874128045401;
Kf2.rotMatUnscaled(2, 0) = 0.980375423909730;
Kf2.rotMatUnscaled(2, 1) = 0.134166713849142;
Kf2.rotMatUnscaled(2, 2) = 0.144441410574456;
Kf2.rotMatUnscaled = Kf1.rotMatUnscaled * Kf2.rotMatUnscaled;
Kf2.transVec = Kf1.rotMatUnscaled * (Eigen::Vector3f(1.128942737114943e+03 / 1000.0f, -1.432967440937596e+02 / 1000.0f, 7.893658796324055e+02 / 1000.0f)) + Kf1.transVec;
Kf2.scale = 1;
Kf2.intr = Intr(366.507, 366.507, 259.864, 206.676);
FrameInfo Kf3;
//Kf.rotMatUnscaled << 1, 0, 0, 0, -1, 0, 0, 0, 1;
//Kf.transVec = Eigen::Vector3f(0,0,0);
Kf3.rotMatUnscaled = Eigen::Matrix3f::Identity();
Kf3.rotMatUnscaled(0, 0) = -0.993943048364607;
Kf3.rotMatUnscaled(0, 1) = 0.030325927333675;
Kf3.rotMatUnscaled(0, 2) = -0.105629327078354;
Kf3.rotMatUnscaled(1, 0) = 0.004398346757531;
Kf3.rotMatUnscaled(1, 1) = 0.971379193082247;
Kf3.rotMatUnscaled(1, 2) = 0.237493405787788;
Kf3.rotMatUnscaled(2, 0) = 0.109808338269340;
Kf3.rotMatUnscaled(2, 1) = 0.235590325306951;
Kf3.rotMatUnscaled(2, 2) = -0.965628980234278;
Kf3.rotMatUnscaled = Kf1.rotMatUnscaled * Kf3.rotMatUnscaled;
Kf3.transVec = Kf1.rotMatUnscaled * (Eigen::Vector3f(2.507968161837474e+02 / 1000.0f, -3.275127035049086e+02 / 1000.0f, 2.057818523590235e+03 / 1000.0f) )+ Kf1.transVec;
Kf3.scale = 1;
Kf3.intr = Intr(366.996, 366.996, 256.466, 209.012);
FrameInfo Kf4;
Kf4.rotMatUnscaled = Eigen::Matrix3f::Identity();
Kf4.rotMatUnscaled(0, 0) = -0.073896009032637;
Kf4.rotMatUnscaled(0, 1) = -0.155250763238798;
Kf4.rotMatUnscaled(0, 2) = 0.985107395344700;
Kf4.rotMatUnscaled(1, 0) = 0.158951733970376;
Kf4.rotMatUnscaled(1, 1) = 0.973346494691277;
Kf4.rotMatUnscaled(1, 2) = 0.165320741408982;
Kf4.rotMatUnscaled(2, 0) = -0.984517001436166;
Kf4.rotMatUnscaled(2, 1) = 0.168801071637521;
Kf4.rotMatUnscaled(2, 2) = -0.047249043346561;
Kf4.rotMatUnscaled = Kf1.rotMatUnscaled * Kf4.rotMatUnscaled;
Kf4.transVec = Kf1.rotMatUnscaled * ( Eigen::Vector3f(-8.272323915033728e+02 / 1000.0f, -1.737204056866098e+02 / 1000.0f, 1.256884341974619e+03 / 1000.0f)) + Kf1.transVec;
Kf4.scale = 1;
Kf4.intr = Intr(366.604, 366.604, 257.112, 198.123);
evoRec.processKeyFrame("test_tsdf_data_4/022718143547_undistorted_1_1.bin","test_tsdf_data_4/022718143547_registered_1_1.jpg", Kf1);
//evoRec.processKeyFrame("test_tsdf_data_4/023446243547_undistorted_1_1.bin", "test_tsdf_data_4/023446243547_registered_1_1.jpg", Kf2);
//evoRec.processKeyFrame("test_tsdf_data_4/001489661447_undistorted_1_1.bin", "test_tsdf_data_4/001489661447_registered_1_1.jpg", Kf3);
//evoRec.processKeyFrame("test_tsdf_data_4/001496161447_undistorted_1_1.bin", "test_tsdf_data_4/001496161447_registered_1_1.jpg", Kf4);
evoRec.savePointCloud("tsdf_cloud.pcd");
//evoRec.generatePointCloud("test_tsdf_data_4/022718143547_undistorted_1_1.bin","test_tsdf_data_4/022718143547_registered_1_1.jpg", Kf1);
}
