int main()
{
try
{
printf ("Starting publisher...\n");
void *context = zmq_ctx_new ();
void *pub_sock = zmq_socket (context, ZMQ_PUB);
int rc = zmq_bind (pub_sock, "tcp://*:5555");
assert (rc == 0);
while (1)
{
mrpt::poses::CPose3D my_pose(0.5f,0.5f,1.5f ,DEG2RAD(-90.0f),DEG2RAD(0),DEG2RAD(-90.0f) );
printf("Publishing pose...\n");
mrpt::comms::mrpt_send_to_zmq(pub_sock, my_pose);
std::this_thread::sleep_for(100ms);
mrpt::img::CImage my_img(800,600, CH_RGB);
printf("Publishing img...\n");
mrpt::comms::mrpt_send_to_zmq(pub_sock, my_img, 0 /* max_packet_len: 0=no max size */);
std::this_thread::sleep_for(100ms);
}
zmq_close (pub_sock);
zmq_ctx_destroy (context);
return 0;
} catch (std::exception &e)
{
std::cerr << "**Exception**: " << e.what() << std::endl;
return -1;
}
}
TEST(CPose3DInterpolator, interp)
{
using namespace mrpt::poses;
using mrpt::math::TPose3D;
using mrpt::math::CMatrixDouble44;
using mrpt::DEG2RAD;
const mrpt::system::TTimeStamp t0 = mrpt::system::now();
const mrpt::system::TTimeStamp dt = mrpt::system::secondsToTimestamp(0.10);
CPose3DInterpolator pose_path;
pose_path.insert(
t0, TPose3D(1., 2., 3., DEG2RAD(30.0), DEG2RAD(.0), DEG2RAD(.0)));
pose_path.insert(
t0 + 2 * dt, TPose3D(
1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0),
DEG2RAD(.0), DEG2RAD(.0)));
TPose3D interp;
bool valid;
pose_path.interpolate(t0 + dt, interp, valid);
EXPECT_TRUE(valid);
const TPose3D interp_good(
1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), DEG2RAD(.0),
DEG2RAD(.0));
EXPECT_NEAR(
.0, (CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
.array()
.abs()
.sum(),
1e-4);
}
void TPose3D::fromString(const std::string& s)
{
CMatrixDouble m;
if (!m.fromMatlabStringFormat(s))
THROW_EXCEPTION("Malformed expression in ::fromString");
ASSERTMSG_(
m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
x = m.get_unsafe(0, 0);
y = m.get_unsafe(0, 1);
z = m.get_unsafe(0, 2);
yaw = DEG2RAD(m.get_unsafe(0, 3));
pitch = DEG2RAD(m.get_unsafe(0, 4));
roll = DEG2RAD(m.get_unsafe(0, 5));
}
// ------------------------------------------------------
// TestCapture
// ------------------------------------------------------
void TestExtractFeaturesTile()
{
CDisplayWindow wind1,wind2;
CFeatureExtraction fExt;
CFeatureList featsHarris;
CImage img;
string the_img = myDataDir+string("test_image.jpg");
if (!img.loadFromFile(the_img ))
{
cerr << "Cannot load " << the_img << endl;
return;
}
cout << "Loaded test image: " << the_img << endl;
CTicTac tictac;
cout << "Extracting Harris features (tiled)... [f_harris_tiled.txt]";
fExt.options.featsType = featHarris;
fExt.options.harrisOptions.tile_image = true;
tictac.Tic();
fExt.detectFeatures( img, featsHarris );
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
cout << "Detected " << featsHarris.size() << " features in " << endl;
featsHarris.saveToTextFile("f_harris_tiled.txt");
wind1.setWindowTitle("Harris detected features (Tiled image)");
wind1.showTiledImageAndPoints( img, featsHarris );
cout << "Extracting Harris features... [f_harris.txt]";
fExt.options.harrisOptions.tile_image = false;
tictac.Tic();
fExt.detectFeatures( img, featsHarris );
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
featsHarris.saveToTextFile("f_harris.txt");
wind2.setWindowTitle("Harris detected features");
wind2.showTiledImageAndPoints( img, featsHarris );
mrpt::system::pause();
return;
}
/*---------------------------------------------------------------
read_double
---------------------------------------------------------------*/
double CConfigFileBase::read_double(
const std::string& section, const std::string& name, double defaultValue,
bool failIfNotFound) const
{
return atof(
readString(section, name, format("%.16e", defaultValue), failIfNotFound)
.c_str());
}
/*---------------------------------------------------------------
read_float
---------------------------------------------------------------*/
float CConfigFileBase::read_float(
const std::string& section, const std::string& name, float defaultValue,
bool failIfNotFound) const
{
return (float)atof(
readString(section, name, format("%.10e", defaultValue), failIfNotFound)
.c_str());
}
/*---------------------------------------------------------------
read_int
---------------------------------------------------------------*/
int CConfigFileBase::read_int(
const std::string& section, const std::string& name, int defaultValue,
bool failIfNotFound) const
{
return atoi(
readString(section, name, format("%i", defaultValue), failIfNotFound)
.c_str());
}
/*---------------------------------------------------------------
read_uint64_t
---------------------------------------------------------------*/
uint64_t CConfigFileBase::read_uint64_t(
const std::string& section, const std::string& name, uint64_t defaultValue,
bool failIfNotFound) const
{
string s = readString(
section, name, format("%lu", (long unsigned int)defaultValue),
failIfNotFound);
return mrpt::system::os::_strtoull(s.c_str(), nullptr, 0);
}
void TTwist2D::fromString(const std::string& s)
{
CMatrixDouble m;
if (!m.fromMatlabStringFormat(s))
THROW_EXCEPTION("Malformed expression in ::fromString");
ASSERTMSG_(
m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
vx = m.get_unsafe(0, 0);
vy = m.get_unsafe(0, 1);
omega = DEG2RAD(m.get_unsafe(0, 2));
}
void TTwist3D::fromString(const std::string& s)
{
CMatrixDouble m;
if (!m.fromMatlabStringFormat(s))
THROW_EXCEPTION("Malformed expression in ::fromString");
ASSERTMSG_(
m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
for (int i = 0; i < 3; i++) (*this)[i] = m.get_unsafe(0, i);
for (int i = 0; i < 3; i++)
(*this)[3 + i] = DEG2RAD(m.get_unsafe(0, 3 + i));
}
void CConfigFileBase::write(
const std::string& section, const std::string& name, double value,
const int name_padding_width, const int value_padding_width,
const std::string& comment)
{
writeString(
section, name,
format(
((std::abs(value) > 1e-4 && std::abs(value) < 1e4) || value == .0)
? "%f"
: "%e",
value),
name_padding_width, value_padding_width, comment);
}
TEST(CPose3DInterpolator, interp)
{
using namespace mrpt::poses;
using mrpt::DEG2RAD;
using mrpt::math::CMatrixDouble44;
using mrpt::math::TPose3D;
auto t0 = mrpt::Clock::now();
mrpt::Clock::duration dt(std::chrono::milliseconds(100));
CPose3DInterpolator pose_path;
pose_path.insert(
t0, TPose3D(1., 2., 3., DEG2RAD(30.0), DEG2RAD(.0), DEG2RAD(.0)));
pose_path.insert(
t0 + 2 * dt, TPose3D(
1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0),
DEG2RAD(.0), DEG2RAD(.0)));
TPose3D interp;
bool valid;
pose_path.interpolate(t0 + dt, interp, valid);
EXPECT_TRUE(valid);
const TPose3D interp_good(
1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), DEG2RAD(.0),
DEG2RAD(.0));
EXPECT_NEAR(
.0,
(CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
.array()
.abs()
.sum(),
2e-4);
}
/*---------------------------------------------------------------
read_string_first_word
---------------------------------------------------------------*/
std::string CConfigFileBase::read_string_first_word(
const std::string& section, const std::string& name,
const std::string& defaultValue, bool failIfNotFound) const
{
string s = readString(section, name, defaultValue, failIfNotFound);
std::vector<std::string> auxStrs;
mrpt::system::tokenize(s, "[], \t", auxStrs);
if (auxStrs.empty())
{
if (failIfNotFound)
{
THROW_EXCEPTION(format(
"Value '%s' seems to be present in section '%s' but, are "
"all whitespaces??",
name.c_str(), section.c_str()));
}
else
return "";
}
else
return auxStrs[0];
}
// ------------------------------------------------------
// TestExtractFeatures
// ------------------------------------------------------
void TestExtractFeatures()
{
CDisplayWindow wind1,wind2,wind3,wind4,wind5;
CFeatureExtraction fExt;
CFeatureList featsHarris, featsKLT, featsSIFT_Hess, featsSIFT_Lowe, featsSIFT_Vedaldi, featsSURF, featsFAST;
CImage img;
if (!img.loadFromFile(the_img_for_extract_feats ))
{
cerr << "Cannot load " << the_img_for_extract_feats << endl;
return;
}
cout << "Loaded test image: " << endl << the_img_for_extract_feats << endl;
cout << "--------------------------------------------------------------------------" << endl << endl;
CTicTac tictac;
fExt.options.patchSize = 0;
cout << "Detect Harris features... [f_harris.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featHarris;
fExt.detectFeatures( img, featsHarris );
cout << "Detected " << featsHarris.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsHarris.saveToTextFile("f_harris.txt");
wind1.setWindowTitle("Harris detected features");
wind1.showImageAndPoints(img, featsHarris);
cout << "Detect FAST features... [f_fast.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featFAST;
fExt.options.FASTOptions.threshold = 15; //150;
fExt.options.FASTOptions.min_distance = 4;
fExt.options.FASTOptions.use_KLT_response = true;
fExt.detectFeatures( img, featsFAST, 0, 500 /* max num feats */ );
cout << "Detected " << featsFAST.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsFAST.saveToTextFile("f_fast.txt");
wind5.setWindowTitle("FAST detected features");
wind5.showImageAndPoints( img, featsFAST );
cout << "Computing SIFT descriptors only ... [f_harris+sift.txt]" << endl;
tictac.Tic();
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.computeDescriptors( img, featsHarris, descSIFT );
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsHarris.saveToTextFile("f_harris+sift.txt");
cout << "Extracting KLT features... [f_klt.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featKLT;
fExt.options.KLTOptions.threshold = 0.05f;
fExt.options.KLTOptions.radius = 5;
fExt.detectFeatures( img, featsKLT, 0, 10 );
cout << "Detected " << featsKLT.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsKLT.saveToTextFile("f_klt.txt");
wind2.setWindowTitle("KLT detected features");
wind2.showImageAndPoints( img, featsKLT );
cout << "Extracting SIFT features... [f_sift_hess.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featSIFT;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.detectFeatures( img, featsSIFT_Hess );
cout << "Detected " << featsSIFT_Hess.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsSIFT_Hess.saveToTextFile("f_sift_hess.txt");
wind3.setWindowTitle("SIFT Hess detected features");
wind3.showImageAndPoints( img, featsSIFT_Hess );
cout << "Extracting SURF features... [f_surf.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featSURF;
fExt.detectFeatures( img, featsSURF );
cout << "Detected " << featsSURF.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsSURF.saveToTextFile("f_surf.txt");
wind4.setWindowTitle("SURF detected features");
wind4.showImageAndPoints( img, featsSURF );
cout << "Computing spin images descriptors only ... [f_harris+spinimgs.txt]" << endl;
tictac.Tic();
fExt.options.SpinImagesOptions.radius = 13;
fExt.options.SpinImagesOptions.hist_size_distance = 10;
fExt.options.SpinImagesOptions.hist_size_intensity = 10;
fExt.computeDescriptors( img, featsHarris, descSpinImages );
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsHarris.saveToTextFile("f_harris+spinimgs.txt");
mrpt::system::pause();
return;
}
// ------------------------------------------------------
// TestMatchingComparative
// ------------------------------------------------------
void TestMatchingComparative()
{
// Take two images
string imgL = myDataDir + string("imL_p01.jpg"); // Left image
string imgR = myDataDir + string("imR_p01.jpg"); // Right image
CImage im1, im2;
im1.loadFromFile( imgL );
im2.loadFromFile( imgR );
size_t imW = im1.getWidth();
size_t imH = im1.getHeight();
CFeatureExtraction fExt;
fExt.options.featsType = featFAST;
fExt.options.patchSize = 21;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
// Find FAST features
CFeatureList list1, list2;
fExt.detectFeatures( im1, list1, 150 );
// Compute SIFT & SURF descriptors
fExt.computeDescriptors( im1, list1, descSIFT );
fExt.computeDescriptors( im1, list1, descSURF );
fExt.detectFeatures( im2, list2, 150 );
// Compute SIFT & SURF descriptors
fExt.computeDescriptors( im2, list2, descSIFT );
fExt.computeDescriptors( im2, list2, descSURF );
CFeatureList::iterator it1, it2;
for( it1 = list1.begin(); it1 != list1.end(); ++it1 )
im1.cross( (*it1)->x, (*it1)->y, TColor::red, '+');
for( it2 = list2.begin(); it2 != list2.end(); ++it2 )
im2.cross( (*it2)->x, (*it2)->y, TColor::red, '+');
CDisplayWindow win, win2;
win.setPos(0,0);
win2.setPos(0,imH*1.5);
CImage joinimage, copyjoinimage, copyInfoImage;
size_t imW2 = 1280;
size_t imH2 = 150;
CImage infoimage( imW2, imH2, CH_RGB );
joinimage.joinImagesHorz( im1, im2 );
infoimage.filledRectangle( 0, 0, imW2, imH2, TColor(150,150,150) );
infoimage.textOut( 20, imH2-53, "SAD", TColor::blue );
infoimage.textOut( 20, imH2-41, "NCC", TColor::blue );
infoimage.textOut( 20, imH2-29, "SIFT", TColor::blue );
infoimage.textOut( 20, imH2-17, "SURF", TColor::blue );
for( it1 = list1.begin(); it1 != list1.end(); ++it1 )
{
copyInfoImage = infoimage;
copyjoinimage = joinimage;
copyjoinimage.line( (*it1)->x, 0, (*it1)->x, imH, TColor::green ); // Horiz
copyjoinimage.line( (*it1)->x+imW, 0, (*it1)->x+imW, imH, TColor::green ); // Horiz
copyjoinimage.line( 0, (*it1)->y, imW+imW, (*it1)->y, TColor::green ); // Epipolar
copyjoinimage.drawCircle( (*it1)->x, (*it1)->y, 4, TColor::green, 2 ); // Keypoint
copyInfoImage.update_patch( (*it1)->patch, 0, 0 );
bool firstMatch = true;
int cnt = 0;
int px = 80;
double minsad = 1.0, maxncc = 0.0;
float minsiftd = 1.0f, minsurfd = 1.0f;
int idxsad = 0, idxncc = 0, idxsiftd = 0, idxsurfd = 0;
for( it2 = list2.begin(); it2 != list2.end(); ++it2 )
{
if( fabs((*it1)->y-(*it2)->y) <= 1.0 && (*it1)->x > (*it2)->x )
{
// Compute matching with SAD and Correlation and SIFT/SURF?
// Use epipolar constraints
// Compute SAD
double sad = mrpt::vision::computeSAD( (*it1)->patch, (*it2)->patch );
if( sad < minsad )
{
minsad = sad;
idxsad = cnt;
}
// Compute Correlation
double ncc;
size_t u, v;
mrpt::vision::openCV_cross_correlation( (*it1)->patch, (*it2)->patch, u, v, ncc );
if( ncc > maxncc )
{
maxncc = ncc;
idxncc = cnt;
}
// Compute distance between descriptors SIFT
float siftd = (*it1)->descriptorSIFTDistanceTo( *(*it2) );
if( siftd < minsiftd )
{
minsiftd = siftd;
idxsiftd = cnt;
}
// Compute distance between descriptors SIFT
float surfd = (*it1)->descriptorSURFDistanceTo( *(*it2) );
if( surfd < minsurfd )
{
minsurfd = surfd;
idxsurfd = cnt;
}
// Plot images + features + each candidate + difference score
if( firstMatch )
{
copyjoinimage.line( (*it1)->x+imW, 0, (*it1)->x+imW, imH, TColor::green ); // Limit line (only the first time)
firstMatch = false;
} // end-if
copyjoinimage.drawCircle( (*it2)->x+imW, (*it2)->y, 4, TColor::blue, 2 ); // Keypoint
double rx0, rx1, ry0, ry1, tx, ty;
rx0 = (*it2)->x+imW-15;
rx1 = (*it2)->x+imW;
tx = (*it2)->x+imW-13;
if( cnt % 2 )
{
ry0 = (*it2)->y-20;
ry1 = (*it2)->y-10;
ty = (*it2)->y-22;
}
else
{
ry0 = (*it2)->y+10;
ry1 = (*it2)->y+20;
ty = (*it2)->y+8;
}
copyjoinimage.filledRectangle( rx0, ry0, rx1, ry1, TColor(150,150,150) );
copyjoinimage.textOut( tx, ty, format("%d", cnt), TColor::blue );
px = 80+cnt*50;
if( px + fExt.options.patchSize > imW2 )
continue;
copyInfoImage.update_patch( (*it2)->patch, px, 30 );
copyInfoImage.textOut( px, imH2-70, format("%d", cnt), TColor::blue );
copyInfoImage.textOut( px, imH2-53, format("%.2f", sad), TColor::blue );
copyInfoImage.textOut( px, imH2-41, format("%.2f", ncc), TColor::blue );
copyInfoImage.textOut( px, imH2-29, format("%.2f", siftd), TColor::blue );
copyInfoImage.textOut( px, imH2-17, format("%.2f", surfd), TColor::blue );
cnt++;
} // end if
} // end for it2
copyInfoImage.textOut( 80+idxsad*50, imH2-53, format("%.2f", minsad), TColor::green );
copyInfoImage.textOut( 80+idxncc*50, imH2-41, format("%.2f", maxncc), TColor::green );
copyInfoImage.textOut( 80+idxsiftd*50, imH2-29, format("%.2f", minsiftd), TColor::green );
copyInfoImage.textOut( 80+idxsurfd*50, imH2-17, format("%.2f", minsurfd), TColor::green );
win.showImage( copyjoinimage );
win2.showImage( copyInfoImage );
mrpt::system::pause();
} // end for it1
// Save to file
// Check number of good features
} // end TestMatchingComparative
// ----
void TTwist3D::asString(std::string& s) const
{
s = mrpt::format(
"[%f %f %f %f %f %f]", vx, vy, vz, RAD2DEG(wx), RAD2DEG(wy),
RAD2DEG(wz));
}
void TPose3D::asString(std::string& s) const
{
s = mrpt::format(
"[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
RAD2DEG(roll));
}
// ----
void TTwist2D::asString(std::string& s) const
{
s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
}
void CPointCloudFilterByDistance::filter(
/** [in,out] The input pointcloud, which will be modified upon return after
filtering. */
mrpt::maps::CPointsMap* pc,
/** [in] The timestamp of the input pointcloud */
const mrpt::system::TTimeStamp pc_timestamp,
/** [in] If nullptr, the PC is assumed to be given in global coordinates.
Otherwise, it will be transformed from local coordinates to global using
this transformation. */
const mrpt::poses::CPose3D& cur_pc_pose,
/** [in,out] additional in/out parameters */
TExtraFilterParams* params)
{
using namespace mrpt::poses;
using namespace mrpt::math;
using mrpt::square;
MRPT_START;
ASSERT_(pc_timestamp != INVALID_TIMESTAMP);
ASSERT_(pc != nullptr);
CSimplePointsMap::Ptr original_pc =
mrpt::make_aligned_shared<CSimplePointsMap>();
original_pc->copyFrom(*pc);
// 1) Filter:
// ---------------------
const size_t N = pc->size();
std::vector<bool> deletion_mask;
deletion_mask.assign(N, false);
size_t del_count = 0;
// get reference, previous PC:
ASSERT_(options.previous_keyframes >= 1);
bool can_do_filter = true;
std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
{
auto it = m_last_frames.rbegin();
for (int i = 0;
i < options.previous_keyframes && it != m_last_frames.rend();
++i, ++it)
{
prev_pc.push_back(&it->second);
}
}
if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes))
{
can_do_filter = false;
}
else
{
for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
{
if (mrpt::system::timeDifference(
m_last_frames.rbegin()->first, pc_timestamp) >
options.too_old_seconds)
{
can_do_filter = false; // A required keyframe is too old
break;
}
}
}
if (can_do_filter)
{
// Reference poses of each PC:
// Previous: prev_pc.pose
mrpt::aligned_std_vector<CPose3D> rel_poses;
for (int k = 0; k < options.previous_keyframes; ++k)
{
const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
rel_poses.push_back(rel_pose);
}
// The idea is that we can now find matches between pt{i} in time_{k},
// composed with rel_pose
// with the local points in time_{k-1}.
std::vector<TPoint3D> pt_km1(options.previous_keyframes);
for (size_t i = 0; i < N; i++)
{
// get i-th point in time=k:
TPoint3Df ptf_k;
pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
const TPoint3D pt_k = TPoint3D(ptf_k);
// Point, referred to time=k-1 frame of reference
for (int k = 0; k < options.previous_keyframes; ++k)
rel_poses[k].composePoint(pt_k, pt_km1[k]);
// Look for neighbors in "time=k"
std::vector<TPoint3D> neig_k;
std::vector<float> neig_sq_dist_k;
pc->kdTreeNClosestPoint3D(
pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);
// Look for neighbors in "time=k-i"
std::vector<TPoint3D> neig_kmi(options.previous_keyframes);
std::vector<float> neig_sq_dist_kmi(
options.previous_keyframes, std::numeric_limits<float>::max());
for (int k = 0; k < options.previous_keyframes; ++k)
{
for (int prev_tim_idx = 0;
prev_tim_idx < options.previous_keyframes; prev_tim_idx++)
{
if (prev_pc[prev_tim_idx]->pc->size() > 0)
{
prev_pc[prev_tim_idx]->pc->kdTreeClosestPoint3D(
pt_km1[prev_tim_idx], neig_kmi[prev_tim_idx],
neig_sq_dist_kmi[prev_tim_idx]);
}
}
}
// Rule:
// we must have at least 1 neighbor in t=k, and 1 neighbor in t=k-i
const double max_allowed_dist_sq = square(
options.min_dist + options.angle_tolerance * pt_k.norm());
bool ok_total = true;
const bool ok_t =
neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
ok_total = ok_total && ok_t;
for (int k = 0; k < options.previous_keyframes; ++k)
{
const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
ok_total = ok_total && ok_tm1;
}
// Delete?
const bool do_delete = !(ok_total);
deletion_mask[i] = do_delete;
if (do_delete) del_count++;
}
// Remove points:
if ((params == nullptr || params->do_not_delete == false) && N > 0 &&
del_count / double(N) <
options.max_deletion_ratio // If we are deleting too many
// points, it may be that the filter
// is plainly wrong
)
{
pc->applyDeletionMask(deletion_mask);
}
} // we can do filter
if (params != nullptr && params->out_deletion_mask != nullptr)
{
*params->out_deletion_mask = deletion_mask;
}
// 2) Add PC to list
// ---------------------
{
FrameInfo fi;
fi.pc = original_pc;
fi.pose = cur_pc_pose;
m_last_frames[pc_timestamp] = fi;
}
// 3) Remove too-old PCs.
// ---------------------
for (auto it = m_last_frames.begin(); it != m_last_frames.end();)
{
if (mrpt::system::timeDifference(it->first, pc_timestamp) >
options.too_old_seconds)
{
it = m_last_frames.erase(it);
}
else
{
++it;
}
}
MRPT_END;
}
void CMetricMapBuilderRBPF::drawCurrentEstimationToImage(CCanvas* img)
{
using mrpt::round;
unsigned int i, M = mapPDF.particlesCount();
std::deque<TPose3D> path;
unsigned int imgHeight = 0;
MRPT_START
const mrpt::maps::CMultiMetricMap* currentMetricMapEstimation =
mapPDF.getCurrentMostLikelyMetricMap();
ASSERT_(currentMetricMapEstimation->m_gridMaps.size() > 0);
// Find which is the most likely path index:
unsigned int bestPath = 0;
double bestPathLik = -1;
for (i = 0; i < M; i++)
{
if (mapPDF.getW(i) > bestPathLik)
{
bestPathLik = mapPDF.getW(i);
bestPath = i;
}
}
// Compute the length of the paths:
mapPDF.getPath(0, path);
// Adapt the canvas size:
bool alreadyCopiedImage = false;
auto g = currentMetricMapEstimation->m_gridMaps[0];
{
auto* obj = dynamic_cast<CImage*>(img);
if (obj) obj->resize(g->getSizeX(), g->getSizeY(), mrpt::img::CH_GRAY);
}
if (!alreadyCopiedImage)
{
CImage imgGrid;
// grid map as bitmap:
// ----------------------------------
g->getAsImage(imgGrid);
img->drawImage(0, 0, imgGrid);
imgHeight = imgGrid.getHeight();
}
int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
float x_min = g->getXMin();
float y_min = g->getYMin();
float resolution = g->getResolution();
// Paths hypothesis:
// ----------------------------------
/***/
for (i = 0; i <= M; i++)
{
if (i != bestPath || i == M)
{
mapPDF.getPath(i == M ? bestPath : i, path);
size_t nPoses = path.size();
// First point: (0,0)
x2 = round((path[0].x - x_min) / resolution);
y2 = round((path[0].y - y_min) / resolution);
// Draw path in the bitmap:
for (size_t j = 0; j < nPoses; j++)
{
// For next segment
x1 = x2;
y1 = y2;
// Coordinates -> pixels
x2 = round((path[j].x - x_min) / resolution);
y2 = round((path[j].y - y_min) / resolution);
// Draw line:
img->line(
x1, round((imgHeight - 1) - y1), x2,
round((imgHeight - 1) - y2),
i == M ? TColor(0, 0, 0)
: TColor(0x50, 0x50, 0x50), // Color, gray levels,
i == M ? 3 : 1 // Line width
);
}
}
}
MRPT_END
}