// Launch unit test.
EUnitTestResult test()
{ CALL
CPoint3D<Tsint> point;
UT_ASSERT_EQUAL_POINTS(point, 0, 0, 0);
point.set(10, 20, 30);
UT_ASSERT_EQUAL_POINTS(point, 10, 20, 30);
point.negate();
UT_ASSERT_EQUAL_POINTS(point, -10, -20, -30);
point.absolute();
UT_ASSERT_EQUAL_POINTS(point, 10, 20, 30);
point += 20;
UT_ASSERT_EQUAL_POINTS(point, 30, 40, 50);
point -= 50;
UT_ASSERT_EQUAL_POINTS(point, -20, -10, 0);
point.clear();
UT_ASSERT_EQUAL_POINTS(point, 0, 0, 0);
UT_ACCEPT;
}
/*---------------------------------------------------------------
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
}
bool
CMathGeom3D::
TrianglesCentroid(CTriangle3D *triangles, int num_triangles, CPoint3D &cpoint)
{
CPoint3D c;
double a, d = 0;
cpoint.zero();
for (int i = 0; i < num_triangles; ++i) {
c = triangles[i].centroid();
a = triangles[i].area2();
cpoint += a*c;
d += a;
}
if (fabs(d) < CMathGen::EPSILON_E6)
return false;
double id = 1.0/d;
cpoint *= id;
return true;
}
void Pitch(float ang) {
AngX += ang;
CPoint3D rotDir = -(At - P0).Dir();
At = P0 + (At - P0).RotateDeg(ang, Up);
Up = Up.RotateDeg(ang, rotDir);
}
// angle in radians
CPoint3D Rotate(float angle, CPoint3D dir)
{
CPoint3D base = (*this) * cos(angle);
CPoint3D elev = dir.Dir() * sin(angle);
//cout << "orig = " + (*this).ToString() + ", base = " + base.ToString() + ", elev = " + elev.ToString() << "\n";
return base + elev;
}
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;
}
CIntersectionInfo CTriangle::GetIntersection (CRay ray)
{
CPoint3D ptNoIntersection (0.0f, 0.0f, 0.0f, 0.0f); // no intersection by default
CPoint3D ptOfIntersection;
CIntersectionInfo hitInfo;
/***************************************************************
STRATEGY:
There are two major steps when trying to find the point of
intersection of a ray with a triangle, first to see if the ray
intersects the plane of the triangle and then to see if the point
on the plane that the ray intersects with lies within the triangle.
So:
1. Find the point of intersection on the triangle plane.
2. Check if this point lies insdie the plane or not.
3. Set the information into the hitInfo object.
4. Return the hitInfo object.
***************************************************************/
// 1.
ptOfIntersection = PlaneIntersect(ray);
// 2.
if ( !inTriangle(ptOfIntersection) || ptOfIntersection.AtInfinity()) {
return hitInfo;
}
// 3.
hitInfo.SetPointOfIntersection (ptOfIntersection);
hitInfo.SetNormal (GetNormalAt(ptOfIntersection));
hitInfo.SetColor(objColor);
hitInfo.SetTexCoords (GetTexCoords(ptOfIntersection));
// 4.
return hitInfo;
}
void MoveCamera(int direction, float delta) {
CPoint3D v;
switch (direction){
case BACK: delta = -delta;
case FRONT:
v = At - P0;
v.Normalize();
SetPosition(P0 + delta*v);
SetDirection(At + delta*v); break;
case LEFT: delta = -delta;
case RIGHT:
v = CrossProduct(At - P0, Up);
v.Normalize();
SetPosition(P0 + delta*v);
SetDirection(At + delta*v); break;
case DOWN: delta = -delta;
case UP:
v = Up;
v.Normalize();
SetPosition(P0 + delta*v);
SetDirection(At + delta*v); break;
}
Update();
}
CRGBA
CRayMarbleTexture::
value(const CPoint3D &p) const
{
double t1 = scale_*solid_noise_.turbulence(freq_*p, octaves_);
double t = 2*fabs(sin(freq_*p.getX() + t1));
if (t < 1)
return t*c1_ + (1 - t)*c2_;
else {
t -= 1;
return t*c0_ + (1 - t)*c1_;
}
}
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
void SetPosition(float x, float y, float z)
{
P0.Set(x, y, z);
}
void Run_KF_SLAM(CConfigFile& cfgFile, const std::string& rawlogFileName)
{
// The EKF-SLAM class:
// Traits for this KF implementation (2D or 3D)
using traits_t = kfslam_traits<IMPL>;
using ekfslam_t = typename traits_t::ekfslam_t;
ekfslam_t mapping;
// The rawlog file:
// ----------------------------------------
const unsigned int rawlog_offset =
cfgFile.read_int("MappingApplication", "rawlog_offset", 0);
const unsigned int SAVE_LOG_FREQUENCY =
cfgFile.read_int("MappingApplication", "SAVE_LOG_FREQUENCY", 1);
const bool SAVE_DA_LOG =
cfgFile.read_bool("MappingApplication", "SAVE_DA_LOG", true);
const bool SAVE_3D_SCENES =
cfgFile.read_bool("MappingApplication", "SAVE_3D_SCENES", true);
const bool SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool(
"MappingApplication", "SAVE_MAP_REPRESENTATIONS", true);
bool SHOW_3D_LIVE =
cfgFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = cfgFile.read_bool(
"MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);
#if !MRPT_HAS_WXWIDGETS
SHOW_3D_LIVE = false;
#endif
string OUT_DIR = cfgFile.read_string(
"MappingApplication", "logOutput_dir", "OUT_KF-SLAM");
string ground_truth_file =
cfgFile.read_string("MappingApplication", "ground_truth_file", "");
string ground_truth_file_robot = cfgFile.read_string(
"MappingApplication", "ground_truth_file_robot", "");
string ground_truth_data_association = cfgFile.read_string(
"MappingApplication", "ground_truth_data_association", "");
cout << "RAWLOG FILE:" << endl << rawlogFileName << endl;
ASSERT_FILE_EXISTS_(rawlogFileName);
CFileGZInputStream rawlogFile(rawlogFileName);
cout << "---------------------------------------------------" << endl
<< endl;
deleteFilesInDirectory(OUT_DIR);
createDirectory(OUT_DIR);
// Load the config options for mapping:
// ----------------------------------------
mapping.loadOptions(cfgFile);
mapping.KF_options.dumpToConsole();
mapping.options.dumpToConsole();
// debug:
// mapping.KF_options.use_analytic_observation_jacobian = true;
// mapping.KF_options.use_analytic_transition_jacobian = true;
// mapping.KF_options.debug_verify_analytic_jacobians = true;
// Is there ground truth of the robot poses??
CMatrixDouble GT_PATH(0, 0);
if (ground_truth_file_robot.size() && fileExists(ground_truth_file_robot))
{
GT_PATH.loadFromTextFile(ground_truth_file_robot);
ASSERT_(GT_PATH.rows() > 0 && GT_PATH.cols() == 6);
}
// Is there a ground truth file of the data association?
std::map<double, std::vector<int>>
GT_DA; // Map: timestamp -> vector(index in observation -> real index)
mrpt::containers::bimap<int, int> DA2GTDA_indices; // Landmark indices
// bimapping: SLAM DA <--->
// GROUND TRUTH DA
if (!ground_truth_data_association.empty() &&
fileExists(ground_truth_data_association))
{
CMatrixDouble mGT_DA;
mGT_DA.loadFromTextFile(ground_truth_data_association);
ASSERT_ABOVEEQ_(mGT_DA.cols(), 3);
// Convert the loaded matrix into a std::map in GT_DA:
for (int i = 0; i < mGT_DA.rows(); i++)
{
std::vector<int>& v = GT_DA[mGT_DA(i, 0)];
if (v.size() <= mGT_DA(i, 1)) v.resize(mGT_DA(i, 1) + 1);
v[mGT_DA(i, 1)] = mGT_DA(i, 2);
}
cout << "Loaded " << GT_DA.size()
<< " entries from DA ground truth file\n";
}
// Create output file for DA perf:
std::ofstream out_da_performance_log;
{
const std::string f = std::string(
OUT_DIR + std::string("/data_association_performance.log"));
out_da_performance_log.open(f.c_str());
ASSERTMSG_(
out_da_performance_log.is_open(),
std::string("Error writing to: ") + f);
// Header:
out_da_performance_log
<< "% TIMESTAMP INDEX_IN_OBS TruePos "
"FalsePos TrueNeg FalseNeg NoGroundTruthSoIDontKnow \n"
<< "%--------------------------------------------------------------"
"--------------------------------------------------\n";
}
if (SHOW_3D_LIVE)
{
win3d = mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow3D>(
"KF-SLAM live view", 800, 500);
win3d->addTextMessage(
0.01, 0.96, "Red: Estimated path", TColorf(0.8f, 0.8f, 0.8f), 100,
MRPT_GLUT_BITMAP_HELVETICA_10);
win3d->addTextMessage(
0.01, 0.93, "Black: Ground truth path", TColorf(0.8f, 0.8f, 0.8f),
101, MRPT_GLUT_BITMAP_HELVETICA_10);
}
// Create DA-log output file:
std::ofstream out_da_log;
if (SAVE_DA_LOG)
{
const std::string f =
std::string(OUT_DIR + std::string("/data_association.log"));
out_da_log.open(f.c_str());
ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);
// Header:
out_da_log << "% TIMESTAMP INDEX_IN_OBS ID "
" RANGE(m) YAW(rad) PITCH(rad) \n"
<< "%-------------------------------------------------------"
"-------------------------------------\n";
}
// The main loop:
// ---------------------------------------
CActionCollection::Ptr action;
CSensoryFrame::Ptr observations;
size_t rawlogEntry = 0, step = 0;
vector<TPose3D> meanPath; // The estimated path
typename traits_t::posepdf_t robotPose;
const bool is_pose_3d = robotPose.state_length != 3;
std::vector<typename IMPL::landmark_point_t> LMs;
std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
CMatrixDouble fullCov;
CVectorDouble fullState;
CTicTac kftictac;
auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);
for (;;)
{
if (os::kbhit())
{
char pushKey = os::getch();
if (27 == pushKey) break;
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
if (!CRawlog::readActionObservationPair(
rawlogArch, action, observations, rawlogEntry))
break; // file EOF
if (rawlogEntry >= rawlog_offset)
{
// Process the action and observations:
// --------------------------------------------
kftictac.Tic();
mapping.processActionObservation(action, observations);
const double tim_kf_iter = kftictac.Tac();
// Get current state:
// -------------------------------
mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
cout << "Mean pose: " << endl << robotPose.mean << endl;
cout << "# of landmarks in the map: " << LMs.size() << endl;
// Get the mean robot pose as 3D:
const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);
// Build the path:
meanPath.push_back(robotPoseMean3D.asTPose());
// Save mean pose:
if (!(step % SAVE_LOG_FREQUENCY))
{
const auto p = robotPose.mean.asVectorVal();
p.saveToTextFile(
OUT_DIR +
format("/robot_pose_%05u.txt", (unsigned int)step));
}
// Save full cov:
if (!(step % SAVE_LOG_FREQUENCY))
{
fullCov.saveToTextFile(
OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
}
// Generate Data Association log?
if (SAVE_DA_LOG)
{
const typename ekfslam_t::TDataAssocInfo& da =
mapping.getLastDataAssociation();
const CObservationBearingRange::Ptr obs =
observations
->getObservationByClass<CObservationBearingRange>();
if (obs)
{
const CObservationBearingRange* obsRB = obs.get();
const double tim =
mrpt::system::timestampToDouble(obsRB->timestamp);
for (size_t i = 0; i < obsRB->sensedData.size(); i++)
{
auto it = da.results.associations.find(i);
int assoc_ID_in_SLAM;
if (it != da.results.associations.end())
assoc_ID_in_SLAM = it->second;
else
{
// It should be a newly created LM:
auto itNewLM = da.newly_inserted_landmarks.find(i);
if (itNewLM != da.newly_inserted_landmarks.end())
assoc_ID_in_SLAM = itNewLM->second;
else
assoc_ID_in_SLAM = -1;
}
out_da_log << format(
"%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
assoc_ID_in_SLAM,
(double)obsRB->sensedData[i].range,
(double)obsRB->sensedData[i].yaw,
(double)obsRB->sensedData[i].pitch);
}
}
}
// Save report on DA performance:
{
const typename ekfslam_t::TDataAssocInfo& da =
mapping.getLastDataAssociation();
const CObservationBearingRange::Ptr obs =
observations
->getObservationByClass<CObservationBearingRange>();
if (obs)
{
const CObservationBearingRange* obsRB = obs.get();
const double tim =
mrpt::system::timestampToDouble(obsRB->timestamp);
auto itDA = GT_DA.find(tim);
for (size_t i = 0; i < obsRB->sensedData.size(); i++)
{
bool is_FP = false, is_TP = false, is_FN = false,
is_TN = false;
if (itDA != GT_DA.end())
{
const std::vector<int>& vDA = itDA->second;
ASSERT_BELOW_(i, vDA.size());
const int GT_ASSOC = vDA[i];
auto it = da.results.associations.find(i);
if (it != da.results.associations.end())
{
// This observation was assigned the already
// existing LM in the map: "it->second"
// TruePos -> If that LM index corresponds to
// that in the GT (with index mapping):
// mrpt::containers::bimap<int,int>
// DA2GTDA_indices;
// // Landmark indices bimapping: SLAM DA <--->
// GROUND TRUTH DA
if (DA2GTDA_indices.hasKey(it->second))
{
const int slam_asigned_LM_idx =
DA2GTDA_indices.direct(it->second);
if (slam_asigned_LM_idx == GT_ASSOC)
is_TP = true;
else
is_FP = true;
}
else
{
// Is this case possible? Assigned to an
// index not ever seen for the first time
// with a GT....
// Just in case:
is_FP = true;
}
}
else
{
// No pairing, but should be a newly created LM:
auto itNewLM =
da.newly_inserted_landmarks.find(i);
if (itNewLM !=
da.newly_inserted_landmarks.end())
{
const int new_LM_in_SLAM = itNewLM->second;
// Was this really a NEW LM not observed
// before?
if (DA2GTDA_indices.hasValue(GT_ASSOC))
{
// GT says this LM was already observed,
// so it shouldn't appear here as new:
is_FN = true;
}
else
{
// Really observed for the first time:
is_TN = true;
DA2GTDA_indices.insert(
new_LM_in_SLAM, GT_ASSOC);
}
}
else
{
// Not associated neither inserted:
// Shouldn't really never arrive here.
}
}
}
// "% TIMESTAMP INDEX_IN_OBS
// TruePos FalsePos TrueNeg FalseNeg
// NoGroundTruthSoIDontKnow \n"
out_da_performance_log << format(
"%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
(int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
(int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
(int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
}
}
}
// Save map to file representations?
if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
{
mapping.saveMapAndPath2DRepresentationAsMATLABFile(
OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
}
// Save 3D view of the filter state:
if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
{
COpenGLScene::Ptr scene3D =
mrpt::make_aligned_shared<COpenGLScene>();
{
opengl::CGridPlaneXY::Ptr grid =
mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
-1000, 1000, -1000, 1000, 0, 5);
grid->setColor(0.4, 0.4, 0.4);
scene3D->insert(grid);
}
// Robot path:
{
opengl::CSetOfLines::Ptr linesPath =
mrpt::make_aligned_shared<opengl::CSetOfLines>();
linesPath->setColor(1, 0, 0);
TPose3D init_pose;
if (!meanPath.empty())
init_pose = CPose3D(meanPath[0]).asTPose();
int path_decim = 0;
for (auto& it : meanPath)
{
linesPath->appendLine(init_pose, it);
init_pose = it;
if (++path_decim > 10)
{
path_decim = 0;
mrpt::opengl::CSetOfObjects::Ptr xyz =
mrpt::opengl::stock_objects::CornerXYZSimple(
0.3f, 2.0f);
xyz->setPose(CPose3D(it));
scene3D->insert(xyz);
}
}
scene3D->insert(linesPath);
// finally a big corner for the latest robot pose:
{
mrpt::opengl::CSetOfObjects::Ptr xyz =
mrpt::opengl::stock_objects::CornerXYZSimple(
1.0, 2.5);
xyz->setPose(robotPoseMean3D);
scene3D->insert(xyz);
}
// The camera pointing to the current robot pose:
if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
{
win3d->setCameraPointingToPoint(
robotPoseMean3D.x(), robotPoseMean3D.y(),
robotPoseMean3D.z());
}
}
// Do we have a ground truth?
if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
{
opengl::CSetOfLines::Ptr GT_path =
mrpt::make_aligned_shared<opengl::CSetOfLines>();
GT_path->setColor(0, 0, 0);
size_t N =
std::min((int)GT_PATH.rows(), (int)meanPath.size());
if (GT_PATH.cols() == 6)
{
double gtx0 = 0, gty0 = 0, gtz0 = 0;
for (size_t i = 0; i < N; i++)
{
const CPose3D p(
GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));
GT_path->appendLine(
gtx0, gty0, gtz0, p.x(), p.y(), p.z());
gtx0 = p.x();
gty0 = p.y();
gtz0 = p.z();
}
}
else if (GT_PATH.cols() == 3)
{
double gtx0 = 0, gty0 = 0;
for (size_t i = 0; i < N; i++)
{
const CPose2D p(
GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));
GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
gtx0 = p.x();
gty0 = p.y();
}
}
scene3D->insert(GT_path);
}
// Draw latest data association:
{
const typename ekfslam_t::TDataAssocInfo& da =
mapping.getLastDataAssociation();
mrpt::opengl::CSetOfLines::Ptr lins =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
lins->setLineWidth(1.2f);
lins->setColor(1, 1, 1);
for (auto it = da.results.associations.begin();
it != da.results.associations.end(); ++it)
{
const prediction_index_t idxPred = it->second;
// This index must match the internal list of features
// in the map:
typename ekfslam_t::KFArray_FEAT featMean;
mapping.getLandmarkMean(idxPred, featMean);
TPoint3D featMean3D;
traits_t::landmark_to_3d(featMean, featMean3D);
// Line: robot -> landmark:
lins->appendLine(
robotPoseMean3D.x(), robotPoseMean3D.y(),
robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
featMean3D.z);
}
scene3D->insert(lins);
}
// The current state of KF-SLAM:
{
opengl::CSetOfObjects::Ptr objs =
mrpt::make_aligned_shared<opengl::CSetOfObjects>();
mapping.getAs3DObject(objs);
scene3D->insert(objs);
}
if (win3d)
{
mrpt::opengl::COpenGLScene::Ptr& scn =
win3d->get3DSceneAndLock();
scn = scene3D;
// Update text messages:
win3d->addTextMessage(
0.02, 0.02,
format(
"Step %u - Landmarks in the map: %u",
(unsigned int)step, (unsigned int)LMs.size()),
TColorf(1, 1, 1), 0, MRPT_GLUT_BITMAP_HELVETICA_12);
win3d->addTextMessage(
0.02, 0.06,
format(
is_pose_3d
? "Estimated pose: (x y z qr qx qy qz) = %s"
: "Estimated pose: (x y yaw) = %s",
robotPose.mean.asString().c_str()),
TColorf(1, 1, 1), 1, MRPT_GLUT_BITMAP_HELVETICA_12);
static vector<double> estHz_vals;
const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
estHz_vals.push_back(curHz);
if (estHz_vals.size() > 50)
estHz_vals.erase(estHz_vals.begin());
const double meanHz = mrpt::math::mean(estHz_vals);
win3d->addTextMessage(
0.02, 0.10,
format(
"Iteration time: %7ss",
mrpt::system::unitsFormat(tim_kf_iter).c_str()),
TColorf(1, 1, 1), 2, MRPT_GLUT_BITMAP_HELVETICA_12);
win3d->addTextMessage(
0.02, 0.14,
format(
"Execution rate: %7sHz",
mrpt::system::unitsFormat(meanHz).c_str()),
TColorf(1, 1, 1), 3, MRPT_GLUT_BITMAP_HELVETICA_12);
win3d->unlockAccess3DScene();
win3d->repaint();
}
if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
{
// Save to file:
CFileGZOutputStream f(
OUT_DIR +
format("/kf_state_%05u.3Dscene", (unsigned int)step));
mrpt::serialization::archiveFrom(f) << *scene3D;
}
}
// Free rawlog items memory:
// --------------------------------------------
action.reset();
observations.reset();
} // (rawlogEntry>=rawlog_offset)
cout << format(
"\nStep %u - Rawlog entries processed: %i\n", (unsigned int)step,
(unsigned int)rawlogEntry);
step++;
}; // end "while(1)"
// Partitioning experiment: Only for 6D SLAM:
traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);
// Is there ground truth of landmarks positions??
if (ground_truth_file.size() && fileExists(ground_truth_file))
{
CMatrixFloat GT(0, 0);
try
{
GT.loadFromTextFile(ground_truth_file);
}
catch (const std::exception& e)
{
cerr << "Ignoring the following error loading ground truth file: "
<< mrpt::exception_to_str(e) << endl;
}
if (GT.rows() > 0 && !LMs.empty())
{
// Each row has:
// [0] [1] [2] [6]
// x y z ID
CVectorDouble ERRS(0);
for (size_t i = 0; i < LMs.size(); i++)
{
// Find the entry in the GT for this mapped LM:
bool found = false;
for (int r = 0; r < GT.rows(); r++)
{
if (LM_IDs[i] == GT(r, 6))
{
const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
ERRS.push_back(gtPt.distanceTo(
CPoint3D(TPoint3D(LMs[i])))); // All these
// conversions
// are to make it
// work with
// either
// CPoint3D &
// TPoint2D
found = true;
break;
}
}
if (!found)
{
cerr << "Ground truth entry not found for landmark ID:"
<< LM_IDs[i] << endl;
}
}
printf("ERRORS VS. GROUND TRUTH:\n");
printf("Mean Error: %f meters\n", math::mean(ERRS));
printf("Minimum error: %f meters\n", math::minimum(ERRS));
printf("Maximum error: %f meters\n", math::maximum(ERRS));
}
} // end if GT
cout << "********* KF-SLAM finished! **********" << endl;
if (win3d)
{
cout << "\n Close the 3D window to quit the application.\n";
win3d->waitForKey();
}
}
void Set(float x0, float y0, float z0, float px0, float py0, float pz0, float upx, float upy, float upz)
{
P0.Set(x0, y0, z0); At.Set(px0, py0, pz0); Up.Set(upx, upy, upz); AngR = AngY = AngP = 0.0f;
}
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
void SetDirection(float x, float y, float z)
{
At.Set(x, y, z);
}
void Roll(float ang) {
AngZ -= ang;
Up = Up.RotateDeg(ang, Right());
}
void CBaseModel::AdjustScaleAndComputeNormalsToVerts()
{
if (m_Verts.empty())
return;
m_NormalsToVerts.resize(m_Verts.size(), CPoint3D(0, 0, 0));
CPoint3D center(0, 0, 0);
double sumArea(0);
CPoint3D sumNormal(0, 0, 0);
double deta(0);
for (int i = 0; i < (int)m_Faces.size(); ++i)
{
CPoint3D normal = VectorCross(Vert(Face(i)[0]),
Vert(Face(i)[1]),
Vert(Face(i)[2]));
double area = normal.Len();
CPoint3D gravity3 = Vert(Face(i)[0]) + Vert(Face(i)[1]) + Vert(Face(i)[2]);
center += area * gravity3;
sumArea += area;
sumNormal += normal;
deta += gravity3 ^ normal;
normal.x /= area;
normal.y /= area;
normal.z /= area;
for (int j = 0; j < 3; ++j)
{
m_NormalsToVerts[Face(i)[j]] += normal;
}
}
center /= sumArea * 3;
fprintf(stderr,"center %lf %lf %lf\n" , center.x , center.y , center.z );
deta -= 3 * (center ^ sumNormal);
if (true)//deta > 0)
{
for (int i = 0; i < GetNumOfVerts(); ++i)
{
if (fabs(m_NormalsToVerts[i].x)
+ fabs(m_NormalsToVerts[i].y)
+ fabs(m_NormalsToVerts[i].z) >= FLT_EPSILON)
{
m_NormalsToVerts[i].Normalize();
}
}
}
else
{
for (int i = 0; i < GetNumOfFaces(); ++i)
{
int temp = m_Faces[i][0];
m_Faces[i][0] = m_Faces[i][1];
m_Faces[i][1] = temp;
}
for (int i = 0; i < GetNumOfVerts(); ++i)
{
if (fabs(m_NormalsToVerts[i].x)
+ fabs(m_NormalsToVerts[i].y)
+ fabs(m_NormalsToVerts[i].z) >= FLT_EPSILON)
{
double len = m_NormalsToVerts[i].Len();
m_NormalsToVerts[i].x /= -len;
m_NormalsToVerts[i].y /= -len;
m_NormalsToVerts[i].z /= -len;
}
}
}
CPoint3D ptUp(m_Verts[0]);
CPoint3D ptDown(m_Verts[0]);
for (int i = 1; i < GetNumOfVerts(); ++i)
{
if (m_Verts[i].x > ptUp.x)
ptUp.x = m_Verts[i].x;
else if (m_Verts[i].x < ptDown.x)
ptDown.x = m_Verts[i].x;
if (m_Verts[i].y > ptUp.y)
ptUp.y = m_Verts[i].y;
else if (m_Verts[i].y < ptDown.y)
ptDown.y = m_Verts[i].y;
if (m_Verts[i].z > ptUp.z)
ptUp.z = m_Verts[i].z;
else if (m_Verts[i].z < ptDown.z)
ptDown.z = m_Verts[i].z;
}
double maxEdgeLenOfBoundingBox = -1;
if (ptUp.x - ptDown.x > maxEdgeLenOfBoundingBox)
maxEdgeLenOfBoundingBox = ptUp.x - ptDown.x;
if (ptUp.y - ptDown.y > maxEdgeLenOfBoundingBox)
maxEdgeLenOfBoundingBox = ptUp.y - ptDown.y;
if (ptUp.z - ptDown.z > maxEdgeLenOfBoundingBox)
maxEdgeLenOfBoundingBox = ptUp.z - ptDown.z;
m_scale = 2.0 / maxEdgeLenOfBoundingBox;
m_center = center;
m_ptUp = ptUp;
m_ptDown = ptDown;
m_ptUp = (m_ptUp - center) * m_scale;
m_ptDown = (m_ptUp - m_ptDown) * m_scale;
//for (int i = 0; i < (int)m_Verts.size(); ++i)
//{
// m_Verts[i] = (m_Verts[i] - center) * m_scale;
//}
m_scale = 1;
m_center = CPoint3D(0, 0, 0);
}
/*-------------------------------------------------------------
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 generateCylinder(CPoint3D start_p, CPoint3D end_p, vector<CPoint3D>& verts, vector<CBaseModel::CFace>& faces, double radius)
{
vector<CPoint3D> up_points;
vector<CPoint3D> down_points;
CPoint3D axis = (end_p - start_p).Normalize();
CPoint3D p0_vec;
if (axis.equal(CPoint3D(1,0,0))) {
p0_vec = (axis * CPoint3D(0,0,1)).Normalize();
}else{
p0_vec = (axis * CPoint3D(1,0,0)).Normalize();
}
//printf("p0 %lf %lf %lf\n" , p0_vec.x, p0_vec.y, p0_vec.z);
//p0_vec = radius * p0_vec;
//printf("p0 %lf %lf %lf\n" , p0_vec.x, p0_vec.y, p0_vec.z);
CPoint3D p4_vec = -1.0 * p0_vec;
CPoint3D p6_vec = (axis * p0_vec).Normalize();
CPoint3D p2_vec = -1.0 * p6_vec;
CPoint3D p7_vec = (p0_vec + p6_vec).Normalize();
CPoint3D p1_vec = (p0_vec + p2_vec).Normalize();
CPoint3D p3_vec = (p2_vec + p4_vec).Normalize();
CPoint3D p5_vec = (p4_vec + p6_vec).Normalize();
//printf("p0 %lf %lf %lf\n" , p0_vec.x, p0_vec.y, p0_vec.z);
//printf("p7 len %lf p0 len %lf\n" , p7_vec.Len(), p0_vec.Len());
up_points.push_back(p0_vec);
up_points.push_back(p1_vec);
up_points.push_back(p2_vec );
up_points.push_back(p3_vec );
up_points.push_back(p4_vec );
up_points.push_back(p5_vec );
up_points.push_back(p6_vec );
up_points.push_back(p7_vec );
vector<CPoint3D> up_points_2;
for (int i = 0; i < up_points.size(); ++i) {
up_points_2.push_back(up_points[i]);
CPoint3D p = (up_points[i] + up_points[(i+1)%up_points.size()]).Normalize();
up_points_2.push_back(p);
}
swap(up_points, up_points_2);
for (auto& p:up_points) {
p *= radius;
p += start_p;
}
for (auto& p:up_points) {
down_points.push_back(p + end_p - start_p);
}
//FILE* file_cylinder = fopen("cylinder.obj", "w");
//for (auto& p:up_points) {
// fprintf(file_cylinder, "v %lf %lf %lf\n" , p.x, p.y, p.z);
//}
//for (auto& p:down_points) {
// fprintf(file_cylinder, "v %lf %lf %lf\n" , p.x, p.y, p.z);
//}
//for (int i = 0; i < up_points.size(); ++i) {
// int up_v0 = i + 1;
// int up_v1 = (i+1)% up_points.size() + 1;
// int down_v0 = i + up_points.size() + 1;
// int down_v1 = (i+1)%up_points.size() + up_points.size() + 1;
// fprintf(file_cylinder, "f %d %d %d\n" , up_v0, down_v1, up_v1);
// fprintf(file_cylinder, "f %d %d %d\n" , up_v0, down_v0, down_v1);
//}
//fclose(file_cylinder);
int verts_old_sz = verts.size();
for (auto& p:up_points) {
verts.push_back(p);
}
for (auto& p:down_points) {
verts.push_back(p);
}
for (int i = 0; i < up_points.size(); ++i) {
int up_v0 = verts_old_sz + i + 1;
int up_v1 = verts_old_sz + (i+1) % up_points.size() + 1;
int down_v0 = verts_old_sz + i + up_points.size() + 1;
int down_v1 = verts_old_sz + (i+1) %up_points.size() + up_points.size() + 1;
faces.push_back(CBaseModel::CFace(up_v0, down_v1, up_v1));
faces.push_back(CBaseModel::CFace(up_v0, down_v0, down_v1));
}
//for (int i = 0; i < up_points.size(); ++i) {
// int up_v0 = i + 1;
// int up_v1 = (i+1)% up_points.size() + 1;
// int down_v0 = i + up_points.size() + 1;
// int down_v1 = (i+1)%up_points.size() + up_points.size() + 1;
// fprintf(file_cylinder, "f %d %d %d\n" , up_v0, down_v1, up_v1);
// fprintf(file_cylinder, "f %d %d %d\n" , up_v0, down_v0, down_v1);
//}
}
string ToString()
{
return "P0 = " + P0.ToString() + ", At = " + At.ToString() + ", Up = " + Up.ToString() + ", dir = " + (At - P0).ToString();
}
CCamera()
{
P0.Set(0.0f, 0.0f, 0.0f); At.Set(0.0f, 0.0f, 1.0f); Up.Set(0.0f, 1.0f, 0.0f); AngR = AngY = AngP = 0.0f;
}
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();
}
CPoint3D xy2rgb(float x, float y) {
CPoint3D rgb;
hsv2rgb(x, y, 0.7, &rgb.x, &rgb.y, &rgb.z);
rgb = rgb * (minf(0.8, rgb.Abs()) / rgb.Abs());
return rgb;
}