TStereoCamera.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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 "base-precomp.h"  // Precompiled headers

#include <mrpt/utils/TStereoCamera.h>
#include <mrpt/utils/CConfigFileMemory.h>
#include <mrpt/utils/CStream.h>

using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE( TStereoCamera, CSerializable, mrpt::utils )

TStereoCamera::TStereoCamera()
{
}


/**  Save as a config block:
  */
void TStereoCamera::saveToConfigFile(const std::string &section,  mrpt::utils::CConfigFileBase &cfg ) const
{
        // [<SECTION>_LEFT]
        //   ...
        // [<SECTION>_RIGHT]
        //   ...
        // [<SECTION>_LEFT2RIGHT_POSE]
        //  pose_quaternion = [x y z qr qx qy qz]

        leftCamera.saveToConfigFile(section+string("_LEFT"), cfg);
        rightCamera.saveToConfigFile(section+string("_RIGHT"), cfg);

        const CPose3DQuat q = CPose3DQuat(rightCameraPose); // Don't remove this line, it's a safe against future possible bugs if rightCameraPose changes!
        cfg.write(section+string("_LEFT2RIGHT_POSE"), "pose_quaternion",q.asString() );
}

/**  Load all the params from a config source, in the format described in saveToConfigFile()
  */
void TStereoCamera::loadFromConfigFile(const std::string &section,  const mrpt::utils::CConfigFileBase &cfg )
{
        // [<SECTION>_LEFT]
        //   ...
        // [<SECTION>_RIGHT]
        //   ...
        // [<SECTION>_LEFT2RIGHT_POSE]
        //  pose_quaternion = [x y z qr qx qy qz]

        leftCamera.loadFromConfigFile(section+string("_LEFT"), cfg);
        rightCamera.loadFromConfigFile(section+string("_RIGHT"), cfg);
        rightCameraPose.fromString( cfg.read_string(section+string("_LEFT2RIGHT_POSE"), "pose_quaternion","" ) );
}

// WriteToStream
void TStereoCamera::writeToStream( CStream &out, int *version ) const
{
        if( version )
                *version = 1;
        else
        {
            out << leftCamera
            << rightCamera
            << rightCameraPose;
        } // end else
}

// ReadFromStream
void TStereoCamera::readFromStream( CStream &in, int version )
{
        switch( version )
        {
        case 0:
        case 1:
    {
                if (version==0)
                {
                        uint8_t _model;
                        in  >> _model;  // unused now
                }

        in  >> leftCamera
            >> rightCamera
            >> rightCameraPose;
    } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION( version )
        }
}

std::string TStereoCamera::dumpAsText() const
{
        mrpt::utils::CConfigFileMemory cfg;
        saveToConfigFile("",cfg);
        return cfg.getContent();
}