types.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "vision-precomp.h"  // Precompiled headers
 
#include <mrpt/vision/types.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/containers/stl_containers_utils.h>
#include <iostream>
#include <iomanip>
 
using namespace std;
using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::system;
 
// ==================== TSequenceFeatureObservations ====================
/** Saves all entries to a text file, with each line having this format:
 * #FRAME_ID  #FEAT_ID  #PIXEL_X  #PIXEL_Y
  * \sa loadFromTextFile  */
void TSequenceFeatureObservations::saveToTextFile(
        const std::string& filName, bool skipFirstCommentLine) const
{
        MRPT_START
 
        ofstream f(filName.c_str());
        if (!f.is_open())
                THROW_EXCEPTION_FMT("Can't open file: %s", filName.c_str())
 
        if (!skipFirstCommentLine)
                f << "% FRAME_ID  FEAT_ID   X         Y     \n"
                         "%-------------------------------------\n";
 
        for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
                f << setw(7) << it->id_frame << setw(7) << it->id_feature << setw(13)
                  << it->px.x << setw(11) << it->px.y << endl;
 
        MRPT_END
}
 
/** Load from a text file, in the format described in \a saveToTextFile */
void TSequenceFeatureObservations::loadFromTextFile(const std::string& filName)
{
        MRPT_START
 
        BASE::clear();  // Erase previous contents
 
        ifstream f(filName.c_str());
        if (!f.is_open())
                THROW_EXCEPTION_FMT("Can't open file: %s", filName.c_str())
 
        unsigned int linNum = 0;
        while (!f.fail())
        {
                linNum++;  // Line counter (for error reporting)
                string lin;
                std::getline(f, lin);
                lin = trim(lin);
                if (lin.empty() || lin[0] == '%') continue;
 
                // Read from each line as a stream:
                std::istringstream s;
                s.str(lin);
 
                TFeatureID featID;
                TCameraPoseID camID;
                TPixelCoordf px;
                if (!(s >> camID >> featID >> px.x >> px.y))
                        THROW_EXCEPTION(
                                format(
                                        "%s:%u: Error parsing line: '%s'", filName.c_str(), linNum,
                                        lin.c_str()))
 
                BASE::push_back(TFeatureObservation(featID, camID, px));
        }
 
        MRPT_END
}
 
bool TSequenceFeatureObservations::saveAsSBAFiles(
        const TLandmarkLocationsVec& pts, const std::string& pts_file,
        const TFramePosesVec& cams, const std::string& cams_file) const
{
        MRPT_START
 
        std::map<TLandmarkID, std::map<TCameraPoseID, TPixelCoordf>> obs_by_point;
 
        for (size_t i = 0; i < BASE::size(); i++)
        {
                const TFeatureObservation& o = (*this)[i];
                std::map<TCameraPoseID, TPixelCoordf>& m = obs_by_point[o.id_feature];
                m[o.id_frame] = o.px;
        }
 
        // # X Y Z  nframes  frame0 x0 y0  frame1 x1 y1 ...
        ofstream f(pts_file.c_str());
        if (!f.is_open()) return false;
 
        f << "# X Y Z  nframes  frame0 x0 y0  frame1 x1 y1 ...\n";
        for (std::map<TLandmarkID,
                                  std::map<TCameraPoseID, TPixelCoordf>>::const_iterator it =
                         obs_by_point.begin();
                 it != obs_by_point.end(); ++it)
        {
                const std::map<TCameraPoseID, TPixelCoordf>& m = it->second;
                f << pts[it->first].x << " " << pts[it->first].y << " "
                  << pts[it->first].z << " " << m.size() << " ";
                for (std::map<TCameraPoseID, TPixelCoordf>::const_iterator itO =
                                 m.begin();
                         itO != m.end(); ++itO)
                        f << itO->first << " " << itO->second.x << " " << itO->second.y
                          << " ";
                f << endl;
        }
 
        ofstream fc(cams_file.c_str());
        if (!fc.is_open()) return false;
 
        for (size_t i = 0; i < cams.size(); i++)
        {
                const mrpt::poses::CPose3D& pos = cams[i];
                const mrpt::poses::CPose3DQuat p(pos);
                fc << p.m_quat[0] << " " << p.m_quat[1] << " " << p.m_quat[2] << " "
                   << p.m_quat[3] << " " << p.x() << " " << p.y() << " " << p.z()
                   << endl;
        }
 
        return true;
        MRPT_END
}
 
/** Remove all those features that don't have a minimum number of observations
 * from different camera frame IDs. */
size_t TSequenceFeatureObservations::removeFewObservedFeatures(
        size_t minNumObservations)
{
        MRPT_START
 
        size_t remCount = 0;
 
        // 1st pass: Count total views
        map<TLandmarkID, size_t> numViews;
        for (BASE::iterator it = BASE::begin(); it != BASE::end(); ++it)
                numViews[it->id_feature]++;
 
        // 2nd pass: Remove selected ones:
        for (size_t idx = 0; idx < BASE::size();)
        {
                if (numViews[(*this)[idx].id_feature] < minNumObservations)
                {
                        BASE::erase(BASE::begin() + idx);
                        remCount++;
                }
                else
                        ++idx;
        }
        return remCount;
        MRPT_END
}
 
/** Remove one out of \a decimate_ratio camera frame IDs from the list.
  * \sa After calling this you may want to call \a compressIDs */
void TSequenceFeatureObservations::decimateCameraFrames(
        const size_t decimate_ratio)
{
        ASSERT_ABOVEEQ_(decimate_ratio, 1);
        if (decimate_ratio == 1) return;  // =1 -> Delete no one!
 
        // 1) Make sorted list of frame IDs:
        set<TCameraPoseID> frameIDs;
        for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
                frameIDs.insert(it->id_frame);
 
        // 2) Leave in "frameIDs" just the IDs that will survive:
        for (set<TCameraPoseID>::iterator it = frameIDs.begin();
                 it != frameIDs.end();)
        {
                // Leave one:
                ++it;
                // Remove "decimate_ratio-1"
                for (size_t d = 0; d < decimate_ratio - 1 && it != frameIDs.end(); d++)
                        it = mrpt::containers::erase_return_next(frameIDs, it);
        }
 
        // 3) Make a new list of observations with only the desired data:
        TSequenceFeatureObservations newLst;
        newLst.reserve(BASE::size() / decimate_ratio);
        for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
                if (frameIDs.find(it->id_frame) != frameIDs.end())
                        newLst.push_back(*it);
 
        // Finally, save content in "this":
        this->swap(newLst);
}
 
/** Rearrange frame and feature IDs such as they start at 0 and there are no
 * gaps. */
void TSequenceFeatureObservations::compressIDs(
        std::map<TCameraPoseID, TCameraPoseID>* old2new_camIDs,
        std::map<TLandmarkID, TLandmarkID>* old2new_lmIDs)
{
        // 1st pass: Make list of translation IDs.
        std::map<TCameraPoseID, TCameraPoseID> camIDs;
        std::map<TLandmarkID, TLandmarkID> lmIDs;
 
        for (BASE::const_iterator it = BASE::begin(); it != BASE::end(); ++it)
        {
                const TFeatureID f_ID = it->id_feature;
                const TCameraPoseID c_ID = it->id_frame;
 
                if (lmIDs.find(f_ID) == lmIDs.end())
                {
                        TLandmarkID nextID = lmIDs.size();  // *IMPORTANT* Separate in 2
                        // lines, otherwise lmIDs[] is
                        // called first (!?)
                        lmIDs[f_ID] = nextID;
                }
                if (camIDs.find(c_ID) == camIDs.end())
                {
                        TCameraPoseID nextID = camIDs.size();  // *IMPORTANT* Separate in 2
                        // lines, otherwise camIDs[]
                        // is called first (!?)
                        camIDs[c_ID] = nextID;
                }
        }
 
        // 2nd: Create a new list with the translated IDs:
        const size_t N = BASE::size();
        TSequenceFeatureObservations newLst(N);
        for (size_t i = 0; i < N; ++i)
        {
                newLst[i].id_feature = lmIDs[(*this)[i].id_feature];
                newLst[i].id_frame = camIDs[(*this)[i].id_frame];
                newLst[i].px = (*this)[i].px;
        }
 
        // And save that new list in "this":
        this->swap(newLst);
 
        // Return translations, if requested:
        if (old2new_camIDs) old2new_camIDs->swap(camIDs);
        if (old2new_lmIDs) old2new_lmIDs->swap(lmIDs);
}