/*---------------------------------------------------------------
getMean
Returns an estimate of the pose, (the mean, or mathematical expectation of the
PDF)
---------------------------------------------------------------*/
void CPointPDFSOG::getMean(CPoint3D& p) const
{
size_t N = m_modes.size();
double X = 0, Y = 0, Z = 0;
if (N)
{
CListGaussianModes::const_iterator it;
double sumW = 0;
for (it = m_modes.begin(); it != m_modes.end(); ++it)
{
double w;
sumW += w = exp(it->log_w);
X += it->val.mean.x() * w;
Y += it->val.mean.y() * w;
Z += it->val.mean.z() * w;
}
if (sumW > 0)
{
X /= sumW;
Y /= sumW;
Z /= sumW;
}
}
p.x(X);
p.y(Y);
p.z(Z);
}
/*---------------------------------------------------------------
The operator u'="this"+u is the pose/point compounding operator.
---------------------------------------------------------------*/
CPoint3D CPose2D::operator + (const CPoint3D& u)const
{
update_cached_cos_sin();
return CPoint3D(
m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi,
u.z() );
}
void TestGeometry3D2()
{
CPose3D iniPoseVehicle(745.327749,407.959716,14.851070,-2.985091,0.009412,0.051315);
CPoint3D GPSPosOnVehicle(-0.25,0,0.10);
CPoint3D iniPoseGPS = iniPoseVehicle+GPSPosOnVehicle;
printf("Pose: %.6f,%.6f,%.6f", iniPoseGPS.x(),iniPoseGPS.y(),iniPoseGPS.z());
}
static void func_inv_compose_point(const CArrayDouble<6+3> &x, const double &dummy, CArrayDouble<3> &Y)
{
MRPT_UNUSED_PARAM(dummy);
CPose3D q(x[0],x[1],x[2],x[3],x[4],x[5]);
const CPoint3D p(x[6+0],x[6+1],x[6+2]);
const CPoint3D pp = p-q;
Y[0]=pp.x();
Y[1]=pp.y();
Y[2]=pp.z();
}
/*---------------------------------------------------------------
drawSingleSample
---------------------------------------------------------------*/
void CPointPDFGaussian::drawSingleSample(CPoint3D &outSample) const
{
MRPT_START
vector_double vec;
randomGenerator.drawGaussianMultivariate(vec,cov);
ASSERT_(vec.size()==3);
outSample.x( mean.x() + vec[0] );
outSample.y( mean.y() + vec[1] );
outSample.z( mean.z() + vec[2] );
MRPT_END
}
double poses_test_compose3Dpoint(int a1, int a2)
{
const long N = 500000;
CTicTac tictac;
CPose3D a(1.0,2.0,3.0,DEG2RAD(10),DEG2RAD(50),DEG2RAD(-30));
CPoint3D b(8.0,-5.0,-1.0);
CPoint3D p;
for (long i=0;i<N;i++)
{
p = a+b;
}
double T = tictac.Tac()/N;
dummy_do_nothing_with_string( mrpt::format("%f",p.x()) );
return T;
}
CPose2D::CPose2D(const CPoint3D &o): m_phi(0),m_cossin_uptodate(false) {
this->m_coords[0] = o.x();
this->m_coords[1] = o.y();
normalizePhi();
}
void CIbeoLuxETH::dataCollection()
{
unsigned char state = SearchForAF;
unsigned char msgIn[1], Header[20], ScanListHeader[44], ScanPointData[10];
unsigned int datatype, /*scannumber,*/ numScanpoints, angleTicks, SPlayer, SPdistance; // SPecho;
int SPHangle;
unsigned char msg[32];
// Start TCP-connection to laserscanner
m_client.connect(m_ip, m_port);
// Send filter command
makeCommandHeader(msg);
makeTypeCommand(msg);
m_client.writeAsync(&msg[0], 32);
// Send start command
makeCommandHeader(msg);
makeStartCommand(msg);
m_client.writeAsync(&msg[0], 28);
while(m_run)
{
switch(state)
{
case SearchForAF:
m_client.readAsync(msgIn, 1, 100, 10);
if (msgIn[0] == 0xAF) state = SearchForFE;
break;
case SearchForFE:
m_client.readAsync(msgIn, 1, 100, 10);
if (msgIn[0] == 0xFE) state = SearchForC0; else state = SearchForAF;
break;
case SearchForC0:
m_client.readAsync(msgIn, 1, 100, 10);
if (msgIn[0] == 0xC0) state = SearchForC2; else state = SearchForAF;
break;
case SearchForC2:
m_client.readAsync(msgIn, 1, 100, 10);
if (msgIn[0] == 0xC2) state = PacketFound; else state = SearchForAF;
break;
case PacketFound:
m_client.readAsync(Header, 20, 100, 10);
datatype = Header[10] * 0x100 + Header[11];
switch(datatype)
{
case 0x2030:
// do nothing
state = SearchForAF;
break;
case 0x2221:
// do nothing
state = SearchForAF;
break;
case 0x2805:
// do nothing
state = SearchForAF;
break;
case 0x2020:
// do nothing
state = SearchForAF;
break;
case 0x2202:
state = SaveData;
break;
default:
std::cerr << "UNKNOWN packet of type " << hex << datatype << " received!!\n";
state = SearchForAF;
}
break;
case SaveData:
// Create new observation object pointer
CObservation3DRangeScanPtr newObs = CObservation3DRangeScan::Create();
newObs->hasPoints3D = true;
newObs->maxRange = 200.00;
m_client.readAsync(ScanListHeader, 44, 10, 10);
/*scannumber =
ScanListHeader[1] * 0x100 + ScanListHeader[0]; */
numScanpoints = ScanListHeader[29] * 0x100 + ScanListHeader[28];
angleTicks = ScanListHeader[23] * 0x100 + ScanListHeader[22];
for (unsigned int i = 0; i < numScanpoints; ++i)
{
bool dropPacket = false;
m_client.readAsync(ScanPointData, 10, 10, 10);
SPlayer = ScanPointData[0] & 0x0F; // two lower bits denote layer
// SPecho = ScanPointData[0] >> 4; // two higher bits denote echo
SPHangle = (char)ScanPointData[3] * 0x100 + ScanPointData[2]; // signed INT16 here
SPdistance = ScanPointData[5] * 0x100 + ScanPointData[4];
// Sanity checks
if (SPlayer > 4)
{
dropPacket = true;
//std::cerr << "Invalid layer: " << SPlayer << " should be element of [0,3] Scanpoint dropped.\n";
}
if ((SPHangle < -((int)angleTicks)/2) || (SPHangle > (int)angleTicks/2))
{
dropPacket = true;
//std::cerr << "Invalid horizontal angle: " << (int)-angleTicks/2 << "< " << SPHangle << " <" << angleTicks/2 << " Scanpoint dropped.\n";
}
if ((SPdistance < 30) || (SPdistance > 20000))
{
dropPacket = true;
//std::cerr << "Invalid distance: 30< " << SPdistance << " <20000 Scanpoint dropped.\n";
}
if (!dropPacket)
{
//TODO: Process point information correctly
CPoint3D cartesianPoint = convertToCartesian(
convertLayerToRad(SPlayer), // vertikal coord of scanner
convertTicksToHRad(SPHangle, angleTicks), // horizontal coord of scanner
SPdistance);
// write scanpoint data to observation object
newObs->points3D_x.push_back(cartesianPoint.x());
newObs->points3D_y.push_back(cartesianPoint.y());
newObs->points3D_z.push_back(cartesianPoint.z());
}
} // for
// return observation to framework
appendObservation( newObs );
state = SearchForAF;
break; // SaveData
} // Switch
} // While
// Send stop command
makeCommandHeader(msg);
makeStopCommand(msg);
m_client.writeAsync(&msg[0], 28);
m_client.close();
} // dataCollection
/*-------------------------------------------------------------
processPreviewNone
-------------------------------------------------------------*/
void CSkeletonTracker::processPreviewNone()
{
using namespace mrpt::opengl;
// show skeleton data
if( m_showPreview )
{
if( !m_win )
{
string caption = string("Preview of ") + m_sensorLabel;
m_win = mrpt::gui::CDisplayWindow3D::Create( caption, 800, 600 );
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
scene->insert( CGridPlaneXZ::Create(-3,3,0,5,-1.5 ) );
// set camera parameters
m_win->setCameraElevationDeg(-90);
m_win->setCameraAzimuthDeg(90);
m_win->setCameraZoom(4);
m_win->setCameraPointingToPoint(0,0,0);
// insert initial body
CSetOfObjectsPtr body = CSetOfObjects::Create();
body->setName("body");
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr sph = CSphere::Create(0.03f);
sph->setColor(0,1,0);
sph->setName( jointNames[i] );
body->insert(sph);
}
// insert initial lines
CSetOfLinesPtr lines = CSetOfLines::Create();
lines->setName("lines");
lines->setColor(0,0,1);
body->insert(lines);
scene->insert(body);
m_win->unlockAccess3DScene();
}
if( m_win && m_win->isOpen() )
{
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
{
m_win->addTextMessage( 0.35, 0.9,
"Please, adopt this position",
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL,
0);
// insert translucid dummy and help text (it will go away when measurements are taken)
if( !scene->getByName("dummy") )
{
const double SCALE = 0.8;
const double BODY_RADIUS = 0.22*SCALE;
const double BODY_LENGTH = 0.8*SCALE;
const double ARM_RADIUS = 0.05*SCALE;
const double ARM_LENGTH = 0.4*SCALE;
const double LEG_RADIUS = 0.1*SCALE;
const double LEG_LENGTH = 0.8*SCALE;
const double HEAD_RADIUS = 0.15*SCALE;
const double ALPHA_CH = 0.8;
CSetOfObjectsPtr dummy = CSetOfObjects::Create();
dummy->setName("dummy");
dummy->setPose(math::TPose3D(0,0,0,0,0,DEG2RAD(-90)));
{
// head
CSpherePtr part = CSphere::Create(HEAD_RADIUS);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(0,0,0.5*BODY_LENGTH+HEAD_RADIUS,0,0,0));
dummy->insert(part);
}
{
// body
CCylinderPtr part = CCylinder::Create(BODY_RADIUS,BODY_RADIUS,BODY_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(0,0,-BODY_LENGTH/2,0,0,0));
dummy->insert(part);
}
{
// left arm 0
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(-BODY_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,DEG2RAD(-90),0));
dummy->insert(part);
}
{
// left arm 1
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(-BODY_RADIUS-ARM_LENGTH+ARM_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,0,0));
dummy->insert(part);
}
{
// right arm 0
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(BODY_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,DEG2RAD(90),0));
dummy->insert(part);
}
{
// right arm 1
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(BODY_RADIUS+ARM_LENGTH-ARM_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,0,0));
dummy->insert(part);
}
{
// left leg
CCylinderPtr part = CCylinder::Create(LEG_RADIUS,LEG_RADIUS,LEG_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(-BODY_RADIUS+LEG_RADIUS,0,-(0.5*BODY_LENGTH+LEG_LENGTH),0,0,0));
dummy->insert(part);
}
{
// right leg
CCylinderPtr part = CCylinder::Create(LEG_RADIUS,LEG_RADIUS,LEG_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(BODY_RADIUS-LEG_RADIUS,0,-(0.5*BODY_LENGTH+LEG_LENGTH),0,0,0));
dummy->insert(part);
}
scene->insert(dummy);
} // end-if
else
{
CSetOfObjectsPtr dummy = static_cast<CSetOfObjectsPtr>( scene->getByName("dummy") );
dummy->setVisibility(true);
}
// update joints positions
CSetOfObjectsPtr body = static_cast<CSetOfObjectsPtr>( scene->getByName("body") );
ASSERT_( body )
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr s = static_cast<CSpherePtr>( body->getByName( jointNames[i] ) );
CPoint3D sphPos;
if( i == 0 )
sphPos = CPoint3D(0,0,0);
else
{
m_joint_theta[i] += M_2PI/(10*(NUM_JOINTS-1));
sphPos.x( 0.5*cos( m_joint_theta[i] ) );
sphPos.y( 0.5*sin( m_joint_theta[i] ) );
sphPos.z( 0.0 );
}
s->setPose( sphPos );
s->setColor( 1, 0, 0 );
s->setRadius( i == 0 ? 0.07 : 0.03 );
} // end-for
} // end-get3DSceneAndLock
m_win->unlockAccess3DScene();
m_win->forceRepaint();
} // end if
} // end if
void CSkeletonTracker::processPreviewNone()
{
using namespace mrpt::opengl;
// show laser scan
if( m_showPreview )
{
if( !m_win )
{
string caption = string("Preview of ") + m_sensorLabel;
m_win = mrpt::gui::CDisplayWindow3D::Create( caption, 800, 600 );
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
scene->insert( CGridPlaneXZ::Create(-3,3,0,5,-1.5 ) );
// set camera parameters
m_win->setCameraElevationDeg(-90);
m_win->setCameraAzimuthDeg(90);
m_win->setCameraZoom(4);
m_win->setCameraPointingToPoint(0,0,0);
// insert initial body
CSetOfObjectsPtr body = CSetOfObjects::Create();
body->setName("body");
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr sph = CSphere::Create(0.03);
sph->setColor(0,1,0);
sph->setName( jointNames[i] );
body->insert(sph);
}
// insert initial lines
CSetOfLinesPtr lines = CSetOfLines::Create();
lines->setName("lines");
lines->setColor(0,0,1);
body->insert(lines);
scene->insert(body);
m_win->unlockAccess3DScene();
}
if( m_win && m_win->isOpen() )
{
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
{
// update joints positions
CSetOfObjectsPtr body = static_cast<CSetOfObjectsPtr>( scene->getByName("body") );
ASSERT_( body )
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr s = static_cast<CSpherePtr>( body->getByName( jointNames[i] ) );
CPoint3D sphPos;
if( i == 0 )
sphPos = CPoint3D(0,0,0);
else
{
m_joint_theta[i] += M_2PI/(10*(NUM_JOINTS-1));
sphPos.x( 0.5*cos( m_joint_theta[i] ) );
sphPos.y( 0.5*sin( m_joint_theta[i] ) );
sphPos.z( 0.0 );
}
s->setPose( sphPos );
s->setColor( 1, 0, 0 );
s->setRadius( i == 0 ? 0.07 : 0.03 );
} // end-for
} // end-get3DSceneAndLock
m_win->unlockAccess3DScene();
m_win->forceRepaint();
} // end if
} // end if