text:
#include <boost/test/unit_test.hpp>
#include "LtpTimerManager.h"
#include <boost/bind/bind.hpp>
#include <boost/timer/timer.hpp>
#include "Ltp.h"

BOOST_AUTO_TEST_CASE(LtpTimerManagerTestCase)
{
    struct Test {
        const boost::posix_time::time_duration ONE_WAY_LIGHT_TIME;
        const boost::posix_time::time_duration ONE_WAY_MARGIN_TIME;
        boost::asio::io_service m_ioService;
        LtpTimerManager<uint64_t> m_timerManager;

        uint64_t m_numCallbacks;
        std::vector<uint64_t> m_desired_serialNumbers;
        std::vector<uint64_t> m_serialNumbersInCallback;
        int m_testNumber;

        Test() :
            ONE_WAY_LIGHT_TIME(boost::posix_time::milliseconds(100)),
            ONE_WAY_MARGIN_TIME(boost::posix_time::milliseconds(100)),
            m_timerManager(m_ioService, ONE_WAY_LIGHT_TIME, ONE_WAY_MARGIN_TIME, boost::bind(&Test::LtpTimerExpiredCallback, this, boost::placeholders::_1, boost::placeholders::_2))
        {

        }

        void LtpTimerExpiredCallback(uint64_t serialNumber, std::vector<uint8_t> & userData) {
            if (m_testNumber == 1) {
                BOOST_REQUIRE_EQUAL(userData.size(), 0);
                m_serialNumbersInCallback.push_back(serialNumber);
                ++m_numCallbacks;
            }
            else if (m_testNumber = 2) {
                BOOST_REQUIRE(userData == std::vector<uint8_t>({ 1,2,3 }));
                m_serialNumbersInCallback.push_back(serialNumber);
                //std::cout << "sn " << serialNumber << std::endl;
                ++m_numCallbacks;
                if (m_numCallbacks <= 3) {
                    BOOST_REQUIRE(m_timerManager.StartTimer(serialNumber, std::vector<uint8_t>({ 1,2,3 }))); //restart
                }
            }
        }

        void DoTest() {
            m_testNumber = 1;
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<uint64_t>({ 5,10,15 });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i]));
            }
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, m_desired_serialNumbers.size());
            BOOST_REQUIRE(m_desired_serialNumbers == m_serialNumbersInCallback);
        }

        void DoTest2() { //readd serial numbers once
            m_testNumber = 2;
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<uint64_t>({ 5,10,15 });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i], std::vector<uint8_t>({ 1,2,3 })));
            }
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, 6);
            BOOST_REQUIRE(m_serialNumbersInCallback == std::vector<uint64_t>({ 5,10,15,5,10,15 }));
        }

        void Test3TimerExpired(/*const boost::system::error_code& e*/) {
            BOOST_REQUIRE(m_timerManager.DeleteTimer(5)); //keep this call within the ioservice thread
        }
        void DoTest3() { //delete an active timer
            m_testNumber = 1;// 1 is valid
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<uint64_t>({ 5,10,15 });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i]));
            }
            //boost::asio::deadline_timer dt(m_ioService);
            //dt.expires_from_now(boost::posix_time::milliseconds(0));
            //dt.async_wait(boost::bind(&Test::Test3TimerExpired, this, boost::asio::placeholders::error));
            boost::asio::post(m_ioService, boost::bind(&Test::Test3TimerExpired, this));
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, 2);
            BOOST_REQUIRE(m_serialNumbersInCallback == std::vector<uint64_t>({ 10,15 }));
        }
        void Test4TimerExpired(/*const boost::system::error_code& e*/) {
            BOOST_REQUIRE(m_timerManager.DeleteTimer(10)); //keep this call within the ioservice thread
        }
        void DoTest4() { //delete a non-active timer
            m_testNumber = 1;// 1 is valid
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<uint64_t>({ 5,10,15 });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i]));
            }
            //boost::asio::deadline_timer dt(m_ioService);
            //dt.expires_from_now(boost::posix_time::milliseconds(0));
            //dt.async_wait(boost::bind(&Test::Test4TimerExpired, this, boost::asio::placeholders::error));
            boost::asio::post(m_ioService, boost::bind(&Test::Test4TimerExpired, this));
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, 2);
            BOOST_REQUIRE(m_serialNumbersInCallback == std::vector<uint64_t>({ 5,15 }));
        }
    };

    Test t;
    t.DoTest();
    t.DoTest2();
    t.DoTest3();
    t.DoTest4();

    struct TestWithSessionId {
        const boost::posix_time::time_duration ONE_WAY_LIGHT_TIME;
        const boost::posix_time::time_duration ONE_WAY_MARGIN_TIME;
        boost::asio::io_service m_ioService;
        LtpTimerManager<Ltp::session_id_t> m_timerManager;

        uint64_t m_numCallbacks;
        std::vector<Ltp::session_id_t> m_desired_serialNumbers;
        std::vector<Ltp::session_id_t> m_serialNumbersInCallback;
        int m_testNumber;

        TestWithSessionId() :
            ONE_WAY_LIGHT_TIME(boost::posix_time::milliseconds(100)),
            ONE_WAY_MARGIN_TIME(boost::posix_time::milliseconds(100)),
            m_timerManager(m_ioService, ONE_WAY_LIGHT_TIME, ONE_WAY_MARGIN_TIME, boost::bind(&TestWithSessionId::LtpTimerExpiredCallback, this, boost::placeholders::_1, boost::placeholders::_2))
        {

        }

        void LtpTimerExpiredCallback(Ltp::session_id_t serialNumber, std::vector<uint8_t> & userData) {
            if (m_testNumber == 1) {
                BOOST_REQUIRE_EQUAL(userData.size(), 0);
                m_serialNumbersInCallback.push_back(serialNumber);
                ++m_numCallbacks;
            }
            else if (m_testNumber = 2) {
                BOOST_REQUIRE(userData == std::vector<uint8_t>({ 1,2,3 }));
                m_serialNumbersInCallback.push_back(serialNumber);
                //std::cout << "sn " << serialNumber << std::endl;
                ++m_numCallbacks;
                if (m_numCallbacks <= 3) {
                    BOOST_REQUIRE(m_timerManager.StartTimer(serialNumber, std::vector<uint8_t>({ 1,2,3 }))); //restart
                }
            }
        }

        void DoTest() {
            m_testNumber = 1;
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<Ltp::session_id_t>({ Ltp::session_id_t(5,6),Ltp::session_id_t(10,11),Ltp::session_id_t(15,16) });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i]));
            }
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, m_desired_serialNumbers.size());
            BOOST_REQUIRE(m_desired_serialNumbers == m_serialNumbersInCallback);
        }

        void DoTest2() { //readd serial numbers once
            m_testNumber = 2;
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<Ltp::session_id_t>({ Ltp::session_id_t(5,6),Ltp::session_id_t(10,11),Ltp::session_id_t(15,16) });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i], std::vector<uint8_t>({ 1,2,3 })));
            }
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, 6);
            BOOST_REQUIRE(m_serialNumbersInCallback == std::vector<Ltp::session_id_t>({
                Ltp::session_id_t(5,6),Ltp::session_id_t(10,11),Ltp::session_id_t(15,16),
                Ltp::session_id_t(5, 6), Ltp::session_id_t(10, 11), Ltp::session_id_t(15, 16)}));
        }

        void Test3TimerExpired(/*const boost::system::error_code& e*/) {
            BOOST_REQUIRE(m_timerManager.DeleteTimer(Ltp::session_id_t(5, 6))); //keep this call within the ioservice thread
        }
        void DoTest3() { //delete an active timer
            m_testNumber = 1;// 1 is valid
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<Ltp::session_id_t>({ Ltp::session_id_t(5,6),Ltp::session_id_t(10,11),Ltp::session_id_t(15,16) });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i]));
            }
            //boost::asio::deadline_timer dt(m_ioService);
            //dt.expires_from_now(boost::posix_time::milliseconds(0));
            //dt.async_wait(boost::bind(&Test::Test3TimerExpired, this, boost::asio::placeholders::error));
            boost::asio::post(m_ioService, boost::bind(&TestWithSessionId::Test3TimerExpired, this));
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, 2);
            BOOST_REQUIRE(m_serialNumbersInCallback == std::vector<Ltp::session_id_t>({ Ltp::session_id_t(10,11),Ltp::session_id_t(15,16) }));
        }
        void Test4TimerExpired(/*const boost::system::error_code& e*/) {
            BOOST_REQUIRE(m_timerManager.DeleteTimer(Ltp::session_id_t(10,11))); //keep this call within the ioservice thread
        }
        void DoTest4() { //delete a non-active timer
            m_testNumber = 1;// 1 is valid
            m_timerManager.Reset();
            m_ioService.stop();
            m_ioService.reset();
            m_numCallbacks = 0;
            m_serialNumbersInCallback.clear();
            m_desired_serialNumbers = std::vector<Ltp::session_id_t>({ Ltp::session_id_t(5,6),Ltp::session_id_t(10,11),Ltp::session_id_t(15,16) });
            for (std::size_t i = 0; i < m_desired_serialNumbers.size(); ++i) {
                BOOST_REQUIRE(m_timerManager.StartTimer(m_desired_serialNumbers[i]));
            }
            //boost::asio::deadline_timer dt(m_ioService);
            //dt.expires_from_now(boost::posix_time::milliseconds(0));
            //dt.async_wait(boost::bind(&Test::Test4TimerExpired, this, boost::asio::placeholders::error));
            boost::asio::post(m_ioService, boost::bind(&TestWithSessionId::Test4TimerExpired, this));
            m_ioService.run();

            BOOST_REQUIRE_EQUAL(m_numCallbacks, 2);
            BOOST_REQUIRE(m_serialNumbersInCallback == std::vector<Ltp::session_id_t>({ Ltp::session_id_t(5,6),Ltp::session_id_t(15,16) }));
        }
    };

    TestWithSessionId t2;
    t2.DoTest();
    t2.DoTest2();
    t2.DoTest3();
    t2.DoTest4();
}


text:
//  (C) Copyright Steve Cleary, Beman Dawes, Howard Hinnant & John Maddock 2000.
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).
//
//  See http://www.boost.org/libs/type_traits for most recent version including documentation.

#ifndef BOOST_TT_HAS_NOTHROW_CONSTRUCTOR_HPP_INCLUDED
#define BOOST_TT_HAS_NOTHROW_CONSTRUCTOR_HPP_INCLUDED

#include <boost/type_traits/has_trivial_constructor.hpp>

// should be the last #include
#include <boost/type_traits/detail/bool_trait_def.hpp>

namespace boost {

namespace detail{

template <class T>
struct has_nothrow_constructor_imp{
   BOOST_STATIC_CONSTANT(bool, value =
      (::boost::type_traits::ice_or<
         ::boost::has_trivial_constructor<T>::value,
         BOOST_HAS_NOTHROW_CONSTRUCTOR(T)
      >::value));
};

}

BOOST_TT_AUX_BOOL_TRAIT_DEF1(has_nothrow_constructor,T,::boost::detail::has_nothrow_constructor_imp<T>::value)

} // namespace boost

#include <boost/type_traits/detail/bool_trait_undef.hpp>

#endif // BOOST_TT_HAS_NOTHROW_CONSTRUCTOR_HPP_INCLUDED

ext: hpp

max_stars_repo_name: framos-gemini/giapi-glue-cc

max_issues_repo_name: framos-gemini/giapi-glue-cc

max_forks_repo_name: framos-gemini/giapi-glue-cc


text:
#include "TestWindow.h"
#include "planecalib/PlaneCalibSystem.h"
#include "planecalib/HomographyCalibration.h"
#include "planecalib/Map.h"
#include "planecalib/PoseTracker.h"
#include "../PlaneCalibApp.h"
#include <opencv2/imgproc.hpp>
#include <Eigen/Dense>
#include <ceres/ceres.h>

namespace planecalib
{
void TestWindow::showHelp() const
{
	BaseWindow::showHelp();
	MYAPP_LOG << "Just tests.";
}

class ImageInterpolation : public ceres::SizedCostFunction<1, 2> {
public:
	ImageInterpolation(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy);

	virtual bool Evaluate(double const* const* parameters,
		double* residuals,
		double** jacobians) const;

protected:
	const cv::Mat1b &mImg;
	const cv::Mat1s &mImgDx;
	const cv::Mat1s &mImgDy;

	template<class T>
	T interpolate(const cv::Mat_<T> &img, const double x, const double y) const;
};

ImageInterpolation::ImageInterpolation(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy):
	mImg(img), mImgDx(imgDx), mImgDy(imgDy)
{
}

bool ImageInterpolation::Evaluate(double const* const* parameters,
	double* residuals,
	double** jacobians) const
{
	int u0 = (int)parameters[0][0];
	int v0 = (int)parameters[0][1];
	//residuals[0] = mImg(v0, u0);

	double u = parameters[0][0];
	double v = parameters[0][1];
	residuals[0] = interpolate(mImg, u, v);
	if (jacobians)
	{
		//jacobians[0][0] = mImgDx(v0, u0);
		//jacobians[0][1] = mImgDy(v0, u0);
		jacobians[0][0] = interpolate(mImgDx, u, v);
		jacobians[0][1] = interpolate(mImgDy, u, v);
	}
	return true;
}

template<class T>
T ImageInterpolation::interpolate(const cv::Mat_<T> &img, const double x, const double y) const
{
	int x0 = (int)x;
	int y0 = (int)y;

	double dx = x - x0;
	double dy = y - y0;

	int x1 = x0+1;
	if (x1 == img.cols)
		x1--;
	int y1 = y0+1;
	if (y1 == img.rows)
		y1--;

	int v00 = (int)img(y0, x0);
	int v10 = (int)img(y1, x0);
	double v0 = v00*(1 - dy) + v10*dy;

	int v01 = (int)img(y0, x1);
	int v11 = (int)img(y1, x1);
	double v1 = v01*(1 - dy) + v11*dy;

	T val = T(v0*(1-dx)+v1*dx);
	return val;
}

class ErrorClass
{
public:
	ErrorClass(const cv::Mat1b &refImg, const cv::Mat1s &refDx, const cv::Mat1s &refDy, const std::vector<Eigen::Vector3i> &evalPositions, const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy);

	template<class T>
	void evalRaw(const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals, T*validCount)  const;

	template<class T>
	bool operator() (const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals)  const;

	const cv::Mat1b &mRefImg;
	const cv::Mat1s &mRefDx;
	const cv::Mat1s &mRefDy;
	const std::vector<Eigen::Vector3i> &mEvalPositions;
	const cv::Mat1b &mImg;
	const cv::Mat1s &mImgDx;
	const cv::Mat1s &mImgDy;

	ceres::CostFunctionToFunctor<1, 2> mInterp;
};

ErrorClass::ErrorClass(const cv::Mat1b &refImg, const cv::Mat1s &refDx, const cv::Mat1s &refDy, const std::vector<Eigen::Vector3i> &evalPositions, const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy) :
mRefImg(refImg), mRefDx(refDx), mRefDy(refDy), mEvalPositions(evalPositions), mImg(img), mImgDx(imgDx), mImgDy(imgDy), mInterp(new ImageInterpolation(img, imgDx, imgDy))
{
}

template<class T>
bool ErrorClass::operator() (const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals)  const
{
	T validCountT;

	this->evalRaw(paramsA, paramsB, paramsC, residuals, &validCountT);
	for (int i = 0; i < (int)mEvalPositions.size(); i++)
	{
		residuals[i] /= validCountT;
	}

	return true;
}

template<class T>
void ErrorClass::evalRaw(const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals, T*validCountT)  const
{
	Eigen::Map<Eigen::Matrix3Xi> evalPositionsMap((int*)mEvalPositions.data(), 3, mEvalPositions.size());
	Eigen::Matrix<T, 3, Eigen::Dynamic> refPos = evalPositionsMap.cast<T>();

	Eigen::Matrix<T, 3, 3> H;
	//H << T(1), T(0), params[0], T(0), T(1), params[1], T(0), T(0), T(1);
	H << paramsB[0], paramsB[1], paramsA[0], paramsB[2], paramsB[3], paramsA[1], paramsC[0], paramsC[1], paramsC[2];

	Eigen::Matrix<T, 3, Eigen::Dynamic> imgPos3 = H*refPos;

	Eigen::Matrix<T, 2, Eigen::Dynamic> imgPos;
	//Eigen::Matrix<T, 2, 100> imgPos;
	imgPos.resize(2, imgPos3.cols());
	imgPos.row(0) = imgPos3.row(0).array() / imgPos3.row(2).array();
	imgPos.row(1) = imgPos3.row(1).array() / imgPos3.row(2).array();

	int validCount = 0;
	for (int i = 0; i < (int)mEvalPositions.size(); i++)
	{
		T *imgPosi = &imgPos(0, i);

		double imgUd = CeresUtils::ToDouble(imgPosi[0]);
		double imgVd = CeresUtils::ToDouble(imgPosi[1]);
		if (imgUd < 0 || imgVd < 0 || imgUd > mImg.cols - 1 || imgVd > mImg.rows - 1)
		{
			residuals[i] = T(0);
			continue;
		}

		T imgVal;
		mInterp(imgPosi, &imgVal);

		auto &refPosi = mEvalPositions[i];
		T refVal = T(mRefImg(refPosi[1], refPosi[0]));

		residuals[i] = imgVal - refVal;
		validCount++;
	}

	*validCountT = T(validCount);
}

bool TestWindow::init(PlaneCalibApp *app, const Eigen::Vector2i &imageSize)
{
	BaseWindow::init(app, imageSize);

	mRefTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));
	mImgTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));
	mCostTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));
	mEvalPositionsTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));

	loadRefFrame();

	resize();

	return true;
}

void TestWindow::loadRefFrame()
{
	const int kMinGradientSq = 10 * 10;

	mRefKeyframe = mApp->getSystem().getMap().getKeyframes()[0].get();
	mRefImg = mApp->getSystem().getMap().getKeyframes()[0]->getImage(0);

	cv::Sobel(mRefImg, mRefDx, CV_16S, 1, 0, 1);
	cv::Sobel(mRefImg, mRefDy, CV_16S, 0, 1, 1);

	cv::Mat1b evalPosImg;
	evalPosImg.create(mRefImg.size());

	for (Eigen::Vector3i p(0, 0, 1); p[1] < mRefImg.rows; p[1]++)
	{
		for (p[0] = 0; p[0] < mRefImg.cols; p[0]++)
		{
			const auto &dx = mRefDx(p[1], p[0]);
			const auto &dy = mRefDy(p[1], p[0]);
			const auto dmSq = dx*dx + dy*dy;
			if (dmSq > kMinGradientSq)
			{
				mEvalPositions.push_back(p);
				evalPosImg(p[1], p[0]) = (uchar)(255 * 0.3f);
			}
			else
				evalPosImg(p[1], p[0]) = 0;
		}
	}

	mPose = Eigen::Matrix3f::Identity();

	mRefTexture.update(mRefImg);

	mCostTexture.update(mRefImg);

	mEvalPositionsTexture.update(evalPosImg);
}
void TestWindow::alignAffine(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy, Eigen::Matrix3fr &pose, TextureHelper &tex)
{
	//Solver options
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::CGNR;
	//options.dense_linear_algebra_library_type = ceres::LAPACK;

	options.max_num_iterations = 500;
	options.num_threads = 4;
	options.num_linear_solver_threads = 4;
	options.logging_type = ceres::SILENT;
	options.minimizer_progress_to_stdout = false;

	Eigen::Vector2d paramsA;
	Eigen::Vector4d paramsB;
	Eigen::Vector3d paramsC;
	//paramsA << 0,0;
	//paramsB << 1, 0, 0, 1;
	//paramsC << 0, 0, 1;

	Eigen::Matrix3fr pose0;
	//pose0 = Eigen::Matrix3f::Identity();
	pose0 = pose;
	paramsA << pose0(0, 2), pose0(1, 2);
	paramsB << pose0(0, 0), pose0(0, 1), pose0(1, 0), pose0(1, 1);
	paramsC << pose0(2, 0), pose0(2, 1), pose0(2, 2);

	//Problem
	ceres::Problem problem;
	problem.AddParameterBlock(paramsA.data(), paramsA.size());
	problem.AddParameterBlock(paramsB.data(), paramsB.size());
	problem.AddParameterBlock(paramsC.data(), paramsC.size());
	//problem.SetParameterBlockConstant(paramsB.data());
	problem.SetParameterBlockConstant(paramsC.data());

	auto *errorFunc = new ErrorClass(mRefImg, mRefDx, mRefDy, mEvalPositions, img, imgDx, imgDy);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ErrorClass, ceres::DYNAMIC, 2, 4, 3>(
		errorFunc, mEvalPositions.size()),
		NULL, paramsA.data(), paramsB.data(), paramsC.data());

	ceres::Solver::Summary summary;
	{
		ProfileSection s("TestSolve");
		ceres::Solve(options, &problem, &summary);
	}

	MYAPP_LOG << "-----------------Align affine-----------------\n";
	MYAPP_LOG << summary.FullReport();
	MYAPP_LOG << "ParamsA: " << paramsA << "\n";
	MYAPP_LOG << "ParamsB: " << paramsB << "\n";
	MYAPP_LOG << "ParamsC: " << paramsC << "\n";

	pose << paramsB[0], paramsB[1], paramsA[0], paramsB[2], paramsB[3], paramsA[1], paramsC[0], paramsC[1], paramsC[2];

	//Cost
	std::vector<double> residuals;
	double validCount;
	residuals.resize(errorFunc->mEvalPositions.size());
	errorFunc->evalRaw(paramsA.data(), paramsB.data(), paramsC.data(), residuals.data(), &validCount);
	MYAPP_LOG << "Valid count: " << validCount << "\n";

	cv::Mat1b costImg;
	costImg.create(errorFunc->mImg.size());
	costImg.setTo(0);
	for (int i = 0; i < errorFunc->mEvalPositions.size(); i++)
	{
		auto &pos = errorFunc->mEvalPositions[i];
		uchar val = cv::saturate_cast<uchar>(std::abs(residuals[i]) * 50);
		costImg(pos[1], pos[0]) = val;
	}

	tex.update(costImg);
}

void TestWindow::alignHomography(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy, Eigen::Matrix3fr &pose, TextureHelper &tex)
{
	//Solver options
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::CGNR;
	//options.dense_linear_algebra_library_type = ceres::LAPACK;

	options.max_num_iterations = 500;
	options.num_threads = 4;
	options.num_linear_solver_threads = 4;
	options.logging_type = ceres::SILENT;
	options.minimizer_progress_to_stdout = false;

	Eigen::Vector2d paramsA;
	Eigen::Vector4d paramsB;
	Eigen::Vector3d paramsC;
	//paramsA << 0,0;
	//paramsB << 1, 0, 0, 1;
	//paramsC << 0, 0, 1;
	Eigen::Matrix3fr pose0;
	//pose0 = Eigen::Matrix3f::Identity();
	pose0 = pose;
	paramsA << pose0(0, 2), pose0(1, 2);
	paramsB << pose0(0, 0), pose0(0, 1), pose0(1, 0), pose0(1, 1);
	paramsC << pose0(2, 0), pose0(2, 1), pose0(2, 2);

	//Problem
	ceres::Problem problem;
	problem.AddParameterBlock(paramsA.data(), paramsA.size());
	problem.AddParameterBlock(paramsB.data(), paramsB.size());

	//std::vector<int> constantIdx;
	//constantIdx.push_back(2);
	//problem.AddParameterBlock(paramsC.data(), paramsC.size(), new ceres::SubsetParameterization(3,constantIdx));
	problem.AddParameterBlock(paramsC.data(), paramsC.size());

	//problem.SetParameterBlockConstant(paramsB.data());
	//problem.SetParameterBlockConstant(paramsC.data());

	auto *errorFunc = new ErrorClass(mRefImg, mRefDx, mRefDy, mEvalPositions, img, imgDx, imgDy);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ErrorClass, ceres::DYNAMIC, 2, 4, 3>(
		errorFunc, mEvalPositions.size()),
		NULL, paramsA.data(), paramsB.data(), paramsC.data());

	ceres::Solver::Summary summary;
	{
		ProfileSection s("TestSolve");
		ceres::Solve(options, &problem, &summary);
	}

	MYAPP_LOG << "-----------------Align homography-----------------\n";
	MYAPP_LOG << summary.FullReport();
	MYAPP_LOG << "ParamsA: " << paramsA << "\n";
	MYAPP_LOG << "ParamsB: " << paramsB << "\n";
	MYAPP_LOG << "ParamsC: " << paramsC << "\n";

	pose << paramsB[0], paramsB[1], paramsA[0], paramsB[2], paramsB[3], paramsA[1], paramsC[0], paramsC[1], paramsC[2];

	//Cost
	std::vector<double> residuals;
	double validCount;
	residuals.resize(errorFunc->mEvalPositions.size());
	errorFunc->evalRaw(paramsA.data(), paramsB.data(), paramsC.data(), residuals.data(), &validCount);
	MYAPP_LOG << "Valid count: " << validCount << "\n";

	cv::Mat1b costImg;
	costImg.create(errorFunc->mImg.size());
	costImg.setTo(0);
	for (int i = 0; i < errorFunc->mEvalPositions.size(); i++)
	{
		auto &pos = errorFunc->mEvalPositions[i];
		uchar val = cv::saturate_cast<uchar>(std::abs(residuals[i]) * 50);
		costImg(pos[1], pos[0]) = val;
	}

	tex.update(costImg);
}

void TestWindow::updateState()
{
	shared_lock<shared_mutex> lockRead(mApp->getSystem().getMap().getMutex());

	if (mApp->getSystem().getMap().getKeyframes()[0].get() != mRefKeyframe)
		loadRefFrame();

	const TrackingFrame *frame = mApp->getSystem().getTracker().getFrame();
	if (!frame)
		return;

	//mPose = Eigen::Matrix3f::Identity();
	//mPose0 = mPose;

	cv::Mat1b img;
	cv::Mat1s imgDx;
	cv::Mat1s imgDy;

	img = frame->getOriginalPyramid()[0];
	cv::Sobel(img, imgDx, CV_16S, 1, 0, 1);
	cv::Sobel(img, imgDy, CV_16S, 0, 1, 1);

	mImgTexture.update(img);

	//mPoseAffine = mPoseHomography = Eigen::Matrix3fr::Identity();

	//alignAffine(img, imgDx, imgDy, mPoseAffine, mCostAffineTexture);
	alignHomography(img, imgDx, imgDy, mPose, mCostTexture);

	//Corners
	Eigen::Matrix<float, 3, 4> refCorners;

	refCorners <<
		0, 0, mRefImg.cols, mRefImg.cols,
		0, mRefImg.rows, mRefImg.rows, 0,
		1,1,1,1;

	Eigen::Matrix<float, 3, 4> corners3;
	corners3 = mPose.inverse() * refCorners;
	mCornerPos.row(0) = corners3.row(0).array() / corners3.row(2).array();
	mCornerPos.row(1) = corners3.row(1).array() / corners3.row(2).array();
}

void TestWindow::resize()
{
	auto screenSize = UserInterfaceInfo::Instance().getScreenSize();
	mTiler.configDevice(Eigen::Vector2i::Zero(), screenSize, 2, 1);
	mTiler.fillTiles();

	//Tile 0 - Alignment
	float scale = 1.5f;
	Eigen::Vector2i sizeScaled = (scale*mImageSize.cast<float>()).cast<int>();
	Eigen::Vector2f padding = (sizeScaled - mImageSize).cast<float>();
	Eigen::Matrix4f offsetMat;
	offsetMat << 1, 0, 0, padding[0] / 2, 0, 1, 0, padding[1] / 2, 0, 0, 1, 0, 0, 0, 0, 1;

	mTiler.setImageMVP(0, sizeScaled);
	mTiler.multiplyMVP(0, offsetMat);

	//Tile 1 - Cost
	mTiler.setImageMVP(1, mImageSize);

	//Tile 2 - absolute
	int smallImgWidth = (int)(screenSize[0] * 0.2f);
	float imgAspect = (float)mImageSize[0] / mImageSize[1];
	int smallImgHeight = (int)(smallImgWidth / imgAspect);

	Eigen::Vector2i smallImgSize(smallImgWidth, smallImgHeight);
	Eigen::Vector2i smallImgOrigin(screenSize[0] - smallImgSize[0], 0);
	mTiler.addAbsoluteTile(smallImgOrigin, smallImgSize);
	mTiler.setImageMVP(2, mImageSize);
}

void TestWindow::draw()
{
	//Generic vertices
	std::vector<Eigen::Vector4f> vertices;
	std::vector<Eigen::Vector2f> textureCoords;
	TextureShader::CreateVertices(mImageSize, vertices, textureCoords);

	///////////////////////////
	// Homography
	// Alignment
	mTiler.setActiveTile(0);
	mShaders->getTextureWarp().setMVPMatrix(mTiler.getMVP());
	mShaders->getColor().setMVPMatrix(mTiler.getMVP());

	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_DST_ALPHA);
	mShaders->getTextureWarp().renderTextureAsAlpha(mRefTexture.getTarget(), mRefTexture.getId(), Eigen::Matrix3fr::Identity(), StaticColors::Green(), mImageSize);
	mShaders->getTextureWarp().renderTextureAsAlpha(mImgTexture.getTarget(), mImgTexture.getId(), mPose, StaticColors::Red(), mImageSize);

	mShaders->getColor().drawVertices(GL_LINE_LOOP, (Eigen::Vector2f*)mCornerPos.data(), mCornerPos.cols(), StaticColors::Green());

	//Cost
	mTiler.setActiveTile(1);
	mShaders->getTexture().setMVPMatrix(mTiler.getMVP());
	mShaders->getTexture().renderTexture(mCostTexture.getTarget(), mCostTexture.getId(), eutils::FromSize(mCostTexture.getSize()));

	//Absolute
	mTiler.setActiveTile(2);
	mShaders->getTexture().setMVPMatrix(mTiler.getMVP());
	mShaders->getTextureWarp().setMVPMatrix(mTiler.getMVP());
	mShaders->getTexture().renderTexture(mCurrentImageTextureTarget, mCurrentImageTextureId, mImageSize);

	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	mShaders->getTextureWarp().renderTextureAsAlpha(mEvalPositionsTexture.getTarget(), mEvalPositionsTexture.getId(), Eigen::Matrix3fr::Identity(), StaticColors::Blue(), mImageSize);
}

} /* namespace dtslam */

ext: cpp

max_stars_repo_name: joshjo/planecalib

max_issues_repo_name: joshjo/planecalib

max_forks_repo_name: joshjo/planecalib


text:
/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2018, Alliance for Sustainable Energy, LLC. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#include <gtest/gtest.h>

#include "UnitsFixture.hpp"

#include "../UnitFactory.hpp"
#include "../Unit.hpp"
#include "../SIUnit.hpp"
#include "../SIUnit_Impl.hpp"
#include "../IPUnit.hpp"
#include "../IPUnit_Impl.hpp"
#include "../BTUUnit.hpp"
#include "../BTUUnit_Impl.hpp"
#include "../CFMUnit.hpp"
#include "../CFMUnit_Impl.hpp"
#include "../CelsiusUnit.hpp"
#include "../CelsiusUnit_Impl.hpp"
#include "../FahrenheitUnit.hpp"
#include "../FahrenheitUnit_Impl.hpp"

#include "../../core/Exception.hpp"

#include <boost/pointer_cast.hpp>

using openstudio::Unit;
using openstudio::OptionalUnit;
using openstudio::UnitSystem;
using openstudio::SIUnit;
using openstudio::OptionalSIUnit;
using openstudio::IPUnit;
using openstudio::OptionalIPUnit;
using openstudio::BTUUnit;
using openstudio::OptionalBTUUnit;
using openstudio::CFMUnit;
using openstudio::OptionalCFMUnit;
using openstudio::CelsiusUnit;
using openstudio::OptionalCelsiusUnit;
using openstudio::FahrenheitUnit;
using openstudio::OptionalFahrenheitUnit;
using openstudio::UnitFactory;
using openstudio::Exception;

TEST_F(UnitsFixture,UnitFactory_ParseStandardUnits)
{
  LOG(Debug,"UnitFactory_ParseStandardUnits");

  OptionalUnit l1 = UnitFactory::instance().createUnit("m");
  OptionalSIUnit siLength = l1->optionalCast<SIUnit>();
  OptionalIPUnit ipLength = l1->optionalCast<IPUnit>();
  ASSERT_TRUE(siLength);
  ASSERT_FALSE(ipLength);

  ASSERT_THROW(siLength->setBaseUnitExponent("ft",1),Exception);
  ASSERT_THROW(l1->setBaseUnitExponent("ft",1),Exception);
  testStreamOutput("m",*siLength);

  OptionalUnit f1 = UnitFactory::instance().createUnit("kg*m/s^2");
  testStreamOutput("N",*f1);

  // RESOLUTION OF SYSTEM CONFLICTS
  // dumb pass-through because of conflict between SI and IP
  OptionalUnit t1 = UnitFactory::instance().createUnit("s");
  testStreamOutput("s",*t1); // is SI unit, but not reliably

  OptionalUnit t2 = UnitFactory::instance().createUnit("s",UnitSystem::SI);
  OptionalSIUnit t2SI = t2->optionalCast<SIUnit>();
  OptionalIPUnit t2IP = t2->optionalCast<IPUnit>();
  ASSERT_TRUE(t2SI);
  ASSERT_FALSE(t2IP);

  OptionalUnit t3 = UnitFactory::instance().createUnit("s",UnitSystem::IP);
  OptionalSIUnit t3SI = t3->optionalCast<SIUnit>();
  OptionalIPUnit t3IP = t3->optionalCast<IPUnit>();
  ASSERT_FALSE(t3SI);
  ASSERT_TRUE(t3IP);

  OptionalUnit T = UnitFactory::instance().createUnit("C");
  OptionalCelsiusUnit TC = T->optionalCast<CelsiusUnit>();
  OptionalFahrenheitUnit TF = T->optionalCast<FahrenheitUnit>();
  ASSERT_TRUE(TC);
  ASSERT_FALSE(TF);

  T = UnitFactory::instance().createUnit("F");
  TC = T->optionalCast<CelsiusUnit>();
  TF = T->optionalCast<FahrenheitUnit>();
  ASSERT_FALSE(TC);
  ASSERT_TRUE(TF);
}

TEST_F(UnitsFixture,UnitFactory_TestForAndReturnUnitObjects) {

  LOG(Debug,"UnitFactory_TestForAndReturnUnitObjects");

  ASSERT_TRUE(openstudio::getSystem("s") == UnitSystem::SI);
  ASSERT_TRUE(openstudio::createUnit("s",UnitSystem::IP)->system() == UnitSystem::IP);
  ASSERT_TRUE(openstudio::getSystem("kg*m/s^2") == UnitSystem::SI);
  ASSERT_TRUE(openstudio::getSystem("ft") == UnitSystem::IP);

  OptionalUnit f1 = openstudio::createUnit("kg*m/s^2");
  ASSERT_TRUE(f1);
  ASSERT_NO_THROW(f1->cast<SIUnit>());
  EXPECT_TRUE(f1->system() == UnitSystem::SI);
  {
    SCOPED_TRACE("Creation of N from standard string.");
    testStreamOutput("N",*f1);
  }
  f1 = openstudio::createUnit("ft^2");
  EXPECT_TRUE(f1->system() == UnitSystem::IP);
  ASSERT_ANY_THROW(f1->cast<SIUnit>());
  OptionalUnit T1 = openstudio::createUnit("R");
  ASSERT_NO_THROW(T1->cast<IPUnit>());
  Unit mixedUnit = (*f1)*(*T1);
  {
    SCOPED_TRACE("Output of mixed unit multiplication.");
    testStreamOutput("ft^2*R",mixedUnit);
  }

  ASSERT_TRUE(openstudio::getSystem("Btu/h") == UnitSystem::BTU);
  OptionalUnit e1 = openstudio::createUnit("Btu/h");
  ASSERT_TRUE(e1);
  ASSERT_NO_THROW(e1->cast<BTUUnit>());
  BTUUnit btuU = e1->cast<BTUUnit>();
  EXPECT_EQ(1,btuU.baseUnitExponent("Btu"));
  EXPECT_EQ(-1,btuU.baseUnitExponent("h"));

  ASSERT_TRUE(openstudio::getSystem("CFM") == UnitSystem::CFM);
  CFMUnit cfmU = openstudio::createUnit("CFM")->cast<CFMUnit>();
  EXPECT_EQ(3,cfmU.baseUnitExponent("ft"));
  EXPECT_EQ(-1,cfmU.baseUnitExponent("min"));
  EXPECT_EQ("cfm",cfmU.prettyString());

  ASSERT_EQ(UnitSystem::Celsius,openstudio::getSystem("C").value());
  CelsiusUnit TC = openstudio::createUnit("C")->cast<CelsiusUnit>();
  ASSERT_ANY_THROW(openstudio::createUnit("W/K")->cast<CelsiusUnit>());

  ASSERT_FALSE(openstudio::isInSystem("F*ft",UnitSystem::Fahrenheit));
  FahrenheitUnit TF = openstudio::createUnit("F")->cast<FahrenheitUnit>();
}

TEST_F(UnitsFixture,UnitFactory_KitchenSink) {
  // unregistered compound units
  SIUnit eui = openstudio::createUnit("J/m^2",UnitSystem::SI)->cast<SIUnit>();
  testStreamOutput("J/m^2",eui);
  EXPECT_EQ("kg/s^2",eui.standardString(false));
  EXPECT_EQ(0,eui.scale().exponent);

  BTUUnit ipEui = openstudio::createUnit("Btu/ft^2",UnitSystem::BTU)->cast<BTUUnit>();
  testStreamOutput("Btu/ft^2",ipEui);
  EXPECT_EQ("",ipEui.prettyString());
  EXPECT_EQ(0,eui.scale().exponent);

  // scaled atomic units
  SIUnit f1 = openstudio::createUnit("\\muN",UnitSystem::SI)->cast<SIUnit>();
  testStreamOutput("\\muN",f1);
  EXPECT_EQ(-6,f1.scale().exponent);

  // parenthetical scaled units
  f1 = openstudio::createUnit("c(kg*m/s^2)",UnitSystem::SI)->cast<SIUnit>();
  testStreamOutput("cN",f1);
  EXPECT_EQ(-2,f1.scale().exponent);

  // first baseUnit scaled
  eui = openstudio::createUnit("MJ/m^2",UnitSystem::SI)->cast<SIUnit>();
  testStreamOutput("MJ/m^2",eui);
  EXPECT_EQ("kg/s^2",eui.standardString(false));
  EXPECT_EQ(6,eui.scale().exponent);

  ipEui = openstudio::createUnit("kBtu/ft^2",UnitSystem::BTU)->cast<BTUUnit>();
  testStreamOutput("kBtu/ft^2",ipEui);
  EXPECT_EQ("Btu/ft^2",ipEui.standardString(false));
  EXPECT_EQ("",ipEui.prettyString());
  EXPECT_EQ(3,ipEui.scale().exponent);

  // multiple baseUnits scaled
  SIUnit u = openstudio::createUnit("MJ/ms*kK",UnitSystem::SI)->cast<SIUnit>();
  testStreamOutput("MW/K",u);
  EXPECT_EQ(6,u.scale().exponent);
  EXPECT_EQ(1,u.baseUnitExponent("kg"));
  EXPECT_EQ(2,u.baseUnitExponent("m"));
  EXPECT_EQ(-3,u.baseUnitExponent("s"));
  EXPECT_EQ(-1,u.baseUnitExponent("K"));

  // odd bug observed. test added so can fix it.
  std::string unitString = openstudio::extractUnitString("J/J");
  u = openstudio::createUnit(unitString,UnitSystem::SI)->cast<SIUnit>();
  EXPECT_EQ("",u.standardString(false));
  EXPECT_EQ("",u.prettyString());
}

/* Test for bug found in #2900 where it currently returns "1/ftH_{2}O^2" (this is a createUnit problem really...) */
TEST_F(UnitsFixture,UnitFactory_IP_Pressure_wc) {
  {
    OptionalUnit unit = openstudio::createUnit("1/ftH_{2}O");
    ASSERT_TRUE(unit);
    EXPECT_EQ("1/ftH_{2}O", unit->standardString());
  }

  {
    OptionalUnit unit = openstudio::createUnit("1/inH_{2}O");
    ASSERT_TRUE(unit);
    EXPECT_EQ("1/inH_{2}O", unit->standardString());
  }
}


text:
#pragma once

#include <string>

#include <supermarx/reference.hpp>
#include <supermarx/datetime.hpp>
#include <supermarx/confidence.hpp>

#include <boost/fusion/include/adapt_struct.hpp>

namespace supermarx
{
namespace data
{

struct productdetails;

struct productdetailsrecord
{
	reference<productdetails> productdetails_id;

	datetime retrieved_on;
	confidence confidence;
};

}
}

BOOST_FUSION_ADAPT_STRUCT(
		supermarx::data::productdetailsrecord,
		(supermarx::reference<supermarx::data::productdetails>, productdetails_id)
		(supermarx::datetime, retrieved_on)
		(supermarx::confidence, confidence)
)

max_stars_repo_name: Supermarx/common

max_issues_repo_name: Supermarx/common

max_forks_repo_name: Supermarx/common


text:
#include <iostream>
#include <fstream>
#include <vector>
//include ros dep.
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Bool.h>

#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include <tf/transform_broadcaster.h>
//include pcl dep
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
//include opencv and eigen
// #include <eigen3/Eigen/Dense>
#include <Eigen/Eigen>
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <cv_bridge/cv_bridge.h>

//#include <cloud_banchmark/cloud_banchmarkConfig.h>
#include "depth_render.cuh"
#include "quadrotor_msgs/PositionCommand.h"
using namespace cv;
using namespace std;
using namespace Eigen;

int *depth_hostptr;
cv::Mat depth_mat;

//camera param
int width, height;
double fx,fy,cx,cy;

DepthRender depthrender;
ros::Publisher pub_depth;
ros::Publisher pub_color;
ros::Publisher pub_pose;
ros::Publisher pub_pcl_wolrd;

sensor_msgs::PointCloud2 local_map_pcl;
sensor_msgs::PointCloud2 local_depth_pcl;

ros::Subscriber odom_sub;
ros::Subscriber global_map_sub, local_map_sub;

ros::Timer local_sensing_timer, estimation_timer;

bool has_global_map(false);
bool has_local_map(false);
bool has_odom(false);

Matrix4d cam02body;
Matrix4d cam2world;
Eigen::Quaterniond cam2world_quat;
nav_msgs::Odometry _odom;

double sensing_horizon, sensing_rate, estimation_rate;
double _x_size, _y_size, _z_size;
double _gl_xl, _gl_yl, _gl_zl;
double _resolution, _inv_resolution;
int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE;

ros::Time last_odom_stamp = ros::TIME_MAX;
Eigen::Vector3d last_pose_world;

void render_currentpose();
void render_pcl_world();

inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index)
{
    Eigen::Vector3d pt;
    pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl;
    pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl;
    pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl;

    return pt;
};

inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt)
{
    Eigen::Vector3i idx;
    idx(0) = std::min( std::max( int( (pt(0) - _gl_xl) * _inv_resolution), 0), _GLX_SIZE - 1);
    idx(1) = std::min( std::max( int( (pt(1) - _gl_yl) * _inv_resolution), 0), _GLY_SIZE - 1);
    idx(2) = std::min( std::max( int( (pt(2) - _gl_zl) * _inv_resolution), 0), _GLZ_SIZE - 1);

    return idx;
};

void rcvOdometryCallbck(const nav_msgs::Odometry& odom)
{
  /*if(!has_global_map)
    return;*/
  has_odom = true;
  _odom = odom;
  Matrix4d Pose_receive = Matrix4d::Identity();

  Eigen::Vector3d request_position;
  Eigen::Quaterniond request_pose;
  request_position.x() = odom.pose.pose.position.x;
  request_position.y() = odom.pose.pose.position.y;
  request_position.z() = odom.pose.pose.position.z;
  request_pose.x() = odom.pose.pose.orientation.x;
  request_pose.y() = odom.pose.pose.orientation.y;
  request_pose.z() = odom.pose.pose.orientation.z;
  request_pose.w() = odom.pose.pose.orientation.w;
  Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix();
  Pose_receive(0,3) = request_position(0);
  Pose_receive(1,3) = request_position(1);
  Pose_receive(2,3) = request_position(2);

  Matrix4d body_pose = Pose_receive;
  //convert to cam pose
  cam2world = body_pose * cam02body;
  cam2world_quat = cam2world.block<3,3>(0,0);

  last_odom_stamp = odom.header.stamp;

  last_pose_world(0) = odom.pose.pose.position.x;
  last_pose_world(1) = odom.pose.pose.position.y;
  last_pose_world(2) = odom.pose.pose.position.z;

  //publish tf
  /*static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) ));
  transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w()));
  br.sendTransform(tf::StampedTransform(transform, last_odom_stamp, "world", "camera")); //publish transform from world frame to quadrotor frame.*/
}

void pubCameraPose(const ros::TimerEvent & event)
{
  //cout<<"pub cam pose"
  geometry_msgs::PoseStamped camera_pose;
  camera_pose.header = _odom.header;
  camera_pose.header.frame_id = "world";
  camera_pose.pose.position.x = cam2world(0,3);
  camera_pose.pose.position.y = cam2world(1,3);
  camera_pose.pose.position.z = cam2world(2,3);
  camera_pose.pose.orientation.w = cam2world_quat.w();
  camera_pose.pose.orientation.x = cam2world_quat.x();
  camera_pose.pose.orientation.y = cam2world_quat.y();
  camera_pose.pose.orientation.z = cam2world_quat.z();
  pub_pose.publish(camera_pose);
}

void renderSensedPoints(const ros::TimerEvent & event)
{
  if(! has_global_map || ! has_odom) return;
  // if( !has_global_map && !has_local_map) return;
  // if( !has_odom ) return;

  render_currentpose();
//   render_pcl_world();
}

vector<float> cloud_data;
void rcvGlobalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
  if(has_global_map)
    return;

  ROS_WARN("[Depth Simulator] Global Pointcloud received..");
  //load global map
  pcl::PointCloud<pcl::PointXYZ> cloudIn;
  pcl::PointXYZ pt_in;
  //transform map to point cloud format
  pcl::fromROSMsg(pointcloud_map, cloudIn);
  for(int i = 0; i < int(cloudIn.points.size()); i++){
    if (isnan(cloudIn.points[i].x) || isnan(cloudIn.points[i].y) || isnan(cloudIn.points[i].z) ||
      isinf(cloudIn.points[i].x) || isinf(cloudIn.points[i].y) || isinf(cloudIn.points[i].z)) {
      continue;
    }
    pt_in = cloudIn.points[i];
    cloud_data.push_back(pt_in.x);
    cloud_data.push_back(pt_in.y);
    cloud_data.push_back(pt_in.z);
  }
  printf("global map has points: %d.\n", (int)cloud_data.size() / 3 );
  //pass cloud_data to depth render
  depthrender.set_data(cloud_data);
  depth_hostptr = (int*) malloc(width * height * sizeof(int));

  has_global_map = true;
  global_map_sub.shutdown();
}

void rcvLocalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
  //ROS_WARN("Local Pointcloud received..");
  //load local map
  pcl::PointCloud<pcl::PointXYZ> cloudIn;
  pcl::PointXYZ pt_in;
  //transform map to point cloud format
  pcl::fromROSMsg(pointcloud_map, cloudIn);

  if(cloudIn.points.size() == 0) return;
  for(int i = 0; i < int(cloudIn.points.size()); i++){
    pt_in = cloudIn.points[i];
    Eigen::Vector3d pose_pt(pt_in.x, pt_in.y, pt_in.z);
    //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt));
    cloud_data.push_back(pose_pt(0));
    cloud_data.push_back(pose_pt(1));
    cloud_data.push_back(pose_pt(2));
  }
  //printf("local map has points: %d.\n", (int)cloud_data.size() / 3 );
  //pass cloud_data to depth render
  depthrender.set_data(cloud_data);
  depth_hostptr = (int*) malloc(width * height * sizeof(int));

  has_local_map = true;
}

void render_pcl_world()
{
  //for debug purpose
  pcl::PointCloud<pcl::PointXYZ> localMap;
  pcl::PointXYZ pt_in;

  Eigen::Vector4d pose_in_camera;
  Eigen::Vector4d pose_in_world;
  Eigen::Vector3d pose_pt;

  for(int u = 0; u < width; u++)
    for(int v = 0; v < height; v++){
      float depth = depth_mat.at<float>(v,u);

      if(depth == 0.0)
        continue;

      pose_in_camera(0) = (u - cx) * depth / fx;
      pose_in_camera(1) = (v - cy) * depth / fy;
      pose_in_camera(2) = depth;
      pose_in_camera(3) = 1.0;

      pose_in_world = cam2world * pose_in_camera;

      if( (pose_in_world.segment(0,3) - last_pose_world).norm() > sensing_horizon )
          continue;

      pose_pt = pose_in_world.head(3);
      //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt));
      pt_in.x = pose_pt(0);
      pt_in.y = pose_pt(1);
      pt_in.z = pose_pt(2);

      localMap.points.push_back(pt_in);
    }

  localMap.width = localMap.points.size();
  localMap.height = 1;
  localMap.is_dense = true;

  pcl::toROSMsg(localMap, local_map_pcl);
  local_map_pcl.header.frame_id  = "world";
  local_map_pcl.header.stamp     = last_odom_stamp;

  pub_pcl_wolrd.publish(local_map_pcl);
}

void render_currentpose()
{
  double this_time = ros::Time::now().toSec();

  Matrix4d cam_pose = cam2world.inverse();

  double pose[4 * 4];

  for(int i = 0; i < 4; i ++)
    for(int j = 0; j < 4; j ++)
      pose[j + 4 * i] = cam_pose(i, j);

  depthrender.render_pose(pose, depth_hostptr);
  //depthrender.render_pose(cam_pose, depth_hostptr);

  depth_mat = cv::Mat::zeros(height, width, CV_32FC1);
  double min = 0.5;
  double max = 1.0f;
  for(int i = 0; i < height; i++)
  	for(int j = 0; j < width; j++)
  	{
  		float depth = (float)depth_hostptr[i * width + j] / 1000.0f;
  		depth = depth < 500.0f ? depth : 0;
  		max = depth > max ? depth : max;
  		depth_mat.at<float>(i,j) = depth;
  	}
  //ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f);
  //printf("max_depth %lf.\n", max);

  cv_bridge::CvImage out_msg;
  out_msg.header.stamp = last_odom_stamp;
  out_msg.header.frame_id = "camera";
  out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
  out_msg.image = depth_mat.clone();
  pub_depth.publish(out_msg.toImageMsg());

  cv::Mat adjMap;
  // depth_mat.convertTo(adjMap,CV_8UC1, 255 / (max-min), -min);
  depth_mat.convertTo(adjMap,CV_8UC1, 255 /13.0, -min);
  cv::Mat falseColorsMap;
  cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW);
  cv_bridge::CvImage cv_image_colored;
  cv_image_colored.header.frame_id = "depthmap";
  cv_image_colored.header.stamp = last_odom_stamp;
  cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8;
  cv_image_colored.image = falseColorsMap;
  pub_color.publish(cv_image_colored.toImageMsg());
  //cv::imshow("depth_image", adjMap);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pcl_render");
  ros::NodeHandle nh("~");

  nh.getParam("cam_width", width);
  nh.getParam("cam_height", height);
  nh.getParam("cam_fx", fx);
  nh.getParam("cam_fy", fy);
  nh.getParam("cam_cx", cx);
  nh.getParam("cam_cy", cy);
  nh.getParam("sensing_horizon", sensing_horizon);
  nh.getParam("sensing_rate",    sensing_rate);
  nh.getParam("estimation_rate", estimation_rate);

  nh.getParam("map/x_size",     _x_size);
  nh.getParam("map/y_size",     _y_size);
  nh.getParam("map/z_size",     _z_size);

  depthrender.set_para(fx, fy, cx, cy, width, height);

  // cam02body <<  0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
  //               0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
  //               -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
  //               0.0, 0.0, 0.0, 1.0;

  cam02body << 0.0, 0.0, 1.0, 0.0,
              -1.0, 0.0, 0.0, 0.0,
               0.0,-1.0, 0.0, 0.0,
               0.0, 0.0, 0.0, 1.0;

  //init cam2world transformation
  cam2world = Matrix4d::Identity();
  //subscribe point cloud
  global_map_sub = nh.subscribe( "global_map", 1,  rcvGlobalPointCloudCallBack);
  local_map_sub  = nh.subscribe( "local_map",  1,  rcvLocalPointCloudCallBack);
  odom_sub       = nh.subscribe( "odometry",   50, rcvOdometryCallbck   );

  //publisher depth image and color image
  pub_depth = nh.advertise<sensor_msgs::Image>("depth",1000);
  pub_color = nh.advertise<sensor_msgs::Image>("colordepth",1000);
  pub_pose  = nh.advertise<geometry_msgs::PoseStamped>("camera_pose",1000);
  pub_pcl_wolrd = nh.advertise<sensor_msgs::PointCloud2>("rendered_pcl",1);

  double sensing_duration  = 1.0 / sensing_rate;
  double estimate_duration = 1.0 / estimation_rate;

  local_sensing_timer = nh.createTimer(ros::Duration(sensing_duration),  renderSensedPoints);
  estimation_timer    = nh.createTimer(ros::Duration(estimate_duration), pubCameraPose);
  //cv::namedWindow("depth_image",1);

  _inv_resolution = 1.0 / _resolution;

  _gl_xl = -_x_size/2.0;
  _gl_yl = -_y_size/2.0;
  _gl_zl =   0.0;

  _GLX_SIZE = (int)(_x_size * _inv_resolution);
  _GLY_SIZE = (int)(_y_size * _inv_resolution);
  _GLZ_SIZE = (int)(_z_size * _inv_resolution);

  ros::Rate rate(100);
  bool status = ros::ok();
  while(status)
  {
    ros::spinOnce();
    status = ros::ok();
    rate.sleep();
  }
}

max_stars_repo_name: ZJU-FAST-Lab/std-trees

max_issues_repo_name: ZJU-FAST-Lab/std-trees

max_forks_repo_name: ZJU-FAST-Lab/std-trees


text:
/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2017, Riccardo Spezialetti, riccardo.spezialetti@unibo.it
*
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the copyright holder(s) nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*/

#ifndef GENERAL_UTILITIES_IMPL
#define GENERAL_UTILITIES_IMPL

#include <iostream>
#include <map>

#include <boost\algorithm\string.hpp>
#include <boost\lexical_cast.hpp>

#include "general_utilities.h"

namespace kpl
{

	static bool compare_path_by_date(const boost::filesystem::path &p1, const boost::filesystem::path &p2)
	{
		return  boost::filesystem::last_write_time(p1) < boost::filesystem::last_write_time(p2);
	}

	static bool compare_path_by_name(const boost::filesystem::path &p1, const boost::filesystem::path &p2)
	{
		return  p1.string() < p2.string();
	}

	int getAllFilesInFolder(const boost::filesystem::path& path_folder, const std::string extension, std::vector<std::string>& values)
	{

		if (!boost::filesystem::exists(path_folder) || !boost::filesystem::is_directory(path_folder))
		{
			perror("Input directory is not directory.");
			return -1;
		}

		boost::filesystem::directory_iterator it(path_folder);
		boost::filesystem::directory_iterator endit;
		std::vector< boost::filesystem::path> all_path;

		while (it != endit)
		{
			if (boost::filesystem::is_regular_file(*it) && it->path().extension() == extension)
			{

				all_path.push_back(*it);
			}
			++it;
		}
		std::sort(all_path.begin(), all_path.end(), compare_path_by_name);

		values.resize(all_path.size(), "");

		for (int i_path = 0; i_path < all_path.size(); i_path++)
		{
			values[i_path] = (all_path[i_path].filename().string());
		}

		return static_cast<uint32_t>(values.size());
	}

	int readCSVFile(const std::string file_name, std::vector<std::vector<std::string>>& lines)
	{
		std::ifstream file(file_name);

		std::string line;
		std::vector<std::string> split_vector;
		int count = 0;

		if (file.is_open())
		{
			while (!file.eof())
			{
				std::getline(file, line, '\n');
				if (line == "" || count == 0)
				{
					count++;
					continue;
				}

				boost::split(split_vector, line, boost::is_any_of(";"), boost::token_compress_on);
				lines.push_back(split_vector);
			}
		}
		else
		{
			std::cerr << file_name << " file not found" << std::endl;
			return -1;
		}

		file.close();

		return static_cast<uint32_t>(lines.size());
	}

	Eigen::Matrix4f computeGroundTruth(const std::string path, const int id)
	{
		std::string line;

		std::ifstream file(path);
		int offset = 3;
		std::vector<std::string> splitVec;
		int index = id * 5 + offset;
		Eigen::Matrix4f transformationMatrix;

		if (file.is_open())
		{
			int count = 1;
			bool done = false;

			while (count < index)
			{
				std::getline(file, line);
				count++;
			}

			for (int i = 0; i < 4; i++)
			{
				std::getline(file, line);
				boost::split(splitVec, line, boost::is_any_of("\t"), boost::token_compress_on);
				transformationMatrix.row(i) << boost::lexical_cast<float>(splitVec.at(0)),
					boost::lexical_cast<float>(splitVec.at(1)),
					boost::lexical_cast<float>(splitVec.at(2)),
					boost::lexical_cast<float>(splitVec.at(3));
			}
		}
		else
			std::cout << "GroundTruth file not found" << std::endl;

		transformationMatrix.inverse();

		return transformationMatrix;
	}

	void findViews(const boost::filesystem::path& path, const std::string extension, std::map<int, std::string>& ids_view_names)
	{
		std::vector<std::string> views_name;
		if (getAllFilesInFolder(path, extension, views_name))
		{

			int id = 0;
			for (size_t i_v = 0; i_v < views_name.size(); i_v++)
			{

				ids_view_names.insert(std::pair<int, std::string>(id, views_name[i_v]));
				id++;
			}
		}
		else
		{
			std::cerr << "Impossible to find views." << std::endl;
		}
	}

	void getColorForHeatMap(const float intensity, int &r, int &g, int &b)
	{
		const int NUM_COLORS = 5;
		static int colors[NUM_COLORS][3] = { { 0, 0, 255 },{ 0, 255, 255 },{ 0, 255, 0 },{ 255, 255, 0 },{ 255, 0, 0 } };

		int color_start, color_end;

		if (intensity <= 0)
		{
			color_start = color_end = 0;
		}
		else if (intensity >= 1)
		{
			color_start = color_end = NUM_COLORS - 1;
		}
		else
		{
			color_start = floor(intensity * (NUM_COLORS - 1));
			color_end = color_start + 1;
		}

		r = static_cast<int>((colors[color_end][0] - colors[color_start][0]) * intensity + colors[color_start][0]);
		g = static_cast<int>((colors[color_end][1] - colors[color_start][1]) * intensity + colors[color_start][1]);
		b = static_cast<int>((colors[color_end][2] - colors[color_start][2]) * intensity + colors[color_start][2]);

	}
}

#endif // !GENERAL_UTILITIES_IMPL

max_stars_repo_name: CVLAB-Unibo/Keypoint-Learning

max_issues_repo_name: jtpils/Keypoint-Learning

max_forks_repo_name: jtpils/Keypoint-Learning


text:
#include "controllerdialog.h"
#include "ui_controllerdialog.h"
#include "Modeling/World.h"
#include "Simulation/WorldSimulation.h"
#include <iostream>
#include <boost/foreach.hpp>

string toStdString(const QString& s);

ControllerDialog::ControllerDialog(WorldSimulation* _sim,QWidget *parent) :
  QDialog(parent),sim(_sim),world(_sim ? _sim->world : NULL),
    ui(new Ui::ControllerDialog)
{
    ui->setupUi(this);

   ui->cmb_robot->clear();
   if(world==NULL) return;
   for(size_t i=0;i<world->robots.size();i++){
     ui->cmb_robot->addItem(QString(world->robots[i]->name.c_str()),(int)i);
   }
   ui->tableWidget->verticalHeader()->setDefaultSectionSize(20);
   Refresh();
}

ControllerDialog::~ControllerDialog()
{
    delete ui;
    refreshing=0;
}

void ControllerDialog::OnRobotChange(int robot)
{
  if(sim==NULL) return;
  if(sim->robotControllers.empty()) return;

  blockSignals(true);
    refreshing=1;
    settings=sim->robotControllers[robot]->Settings();
    ui->tableWidget->clear();
    ui->tableWidget->setRowCount(settings.size());
    ui->tableWidget->setColumnCount(1);
    int j=0;
    for(map<string,string>::iterator i=settings.begin();i!=settings.end();i++,j++){
        ui->tableWidget->setItem(j,0,new QTableWidgetItem());
        ui->tableWidget->setVerticalHeaderItem(j,new QTableWidgetItem(QString::fromStdString(i->first)));
        ui->tableWidget->item(j,0)->setText(QString::fromStdString(i->second));
    }
    commands=sim->robotControllers[robot]->Commands();
    BOOST_FOREACH(string str,commands){
        ui->comboBox->addItem(QString::fromStdString(str));
    }
    refreshing=0;
  blockSignals(false);
}

void ControllerDialog::Refresh(){
    refreshing=1;
    int index = ui->cmb_robot->itemData(ui->cmb_robot->currentIndex()).toInt();
    OnRobotChange(index);
    refreshing=0;
}

void ControllerDialog::OnCellEdited(QTableWidgetItem* item) {
    if(refreshing) return;
    string key= toStdString(ui->tableWidget->verticalHeaderItem(item->row())->text());
    settings[key]= toStdString(item->text());
    int robot = ui->cmb_robot->itemData(ui->cmb_robot->currentIndex()).toInt();
    emit SendControllerSetting(robot,key,settings[key]);
}

void ControllerDialog::OnSendCommand()
{
  int robot = ui->cmb_robot->itemData(ui->cmb_robot->currentIndex()).toInt();
  if(ui->lineEdit->text().isEmpty()) ui->lineEdit->setFocus();
  else emit ControllerCommand(robot, toStdString(ui->comboBox->currentText()), toStdString(ui->lineEdit->text()));
}

void ControllerDialog::OnConnectSerial()
{
  int robot = ui->cmb_robot->itemData(ui->cmb_robot->currentIndex()).toInt();
    emit MakeConnect(robot,
                     QString("localhost"),ui->spn_port->value(),ui->spn_rate->value());
}

max_stars_repo_name: yuxiang-he/Klampt

max_issues_repo_name: yuxiang-he/Klampt

max_forks_repo_name: yuxiang-he/Klampt


text:
// #pragma GCC optimize ("O0")

#include <boost/log/trivial.hpp>

#include "rtcmEncoder.hpp"
#include "acsConfig.hpp"
#include "constants.hpp"

void calculateSsrComb(
	GTime 						curTime,
	int 						period,
	SSRMeta 					ssrMeta,
	int 						masterIod,
	map<SatSys, SSROut>&		ssrOutMap)
{
	if (ssrOutMap.empty())
	{
		BOOST_LOG_TRIVIAL(warning) << "No suitable Ephemeris data available.";
		return;
	}

	map<E_Sys, std::array<double,2>> commonClockOffsetsMap; // 0: bias; 1: bias rate
	map<E_Sys, std::array<GTime, 2>> commonClockEpochsMap;	// 0: 1st straddle pt epoch; 1: 2nd straddle pt epoch

	for (auto& [Sat, ssrOut] : ssrOutMap)
	{
		if (ssrOut.clkInput.vals[0].iode != ssrOut.ephInput.vals[0].iode)
		{
			tracepdeex(3, std::cout, "IODE mismatch between clock and ephemeris for %s\n", Sat.id().c_str());
			continue;
		}

		auto& ssrEph		= ssrOut.ssrEph;
		auto& ssrClk		= ssrOut.ssrClk;
		auto& ssrEphInput	= ssrOut.ephInput;
		auto& ssrClkInput	= ssrOut.clkInput;

		ssrEph.t0 					= curTime + period / 2;
		ssrClk.t0 					= curTime + period / 2;
		ssrEph.udi					= period;
		ssrClk.udi					= period;
		ssrEph.iod					= masterIod;
		ssrClk.iod					= masterIod;

		ssrEph.ssrMeta				= ssrMeta;
		ssrClk.ssrMeta				= ssrMeta;

		ssrEph.iode		= ssrEphInput.vals[0].iode;

		// ssrEph.iodcrc				= ??

		double		clkCorrections[2];
		Vector3d	posCorrections[2];

		for (int i = 0; i < 2; i++)
		{
			posCorrections[i]	= ssrEphInput.vals[i].brdcPos
								- ssrEphInput.vals[i].precPos;
			clkCorrections[i]	= ssrClkInput.vals[i].brdcClk
								- ssrClkInput.vals[i].precClk;
		}
#if 0
		Vector3d	diffRAC[2];
		double		diffClock[2];

		for (int dt : {0, 1})
		{
			double		ephRatio	= (ssrEph.t0 + dt - ssrEphInput.vals[0].time) / (ssrEphInput.vals[1].time - ssrEphInput.vals[0].time);
			double		clkRatio	= (ssrClk.t0 + dt - ssrClkInput.vals[0].time) / (ssrClkInput.vals[1].time - ssrClkInput.vals[0].time);

			Vector3d	posCorrection	= posCorrections[0] 			+ ephRatio	* (posCorrections[1]			- posCorrections[0]);
			double		clkCorrection	= clkCorrections[0]				+ clkRatio	* (clkCorrections[1]			- clkCorrections[0]);

			Vector3d	satPosition		= ssrEphInput.vals[0].brdcPos	+ ephRatio	* (ssrEphInput.vals[1].brdcPos	- ssrEphInput.vals[0].brdcPos);
			Vector3d	satVelocity		= ssrEphInput.vals[0].brdcVel	+ ephRatio	* (ssrEphInput.vals[1].brdcVel	- ssrEphInput.vals[0].brdcVel);

			Vector3d	diffRac			= ecef2rac(satPosition, satVelocity) * posCorrection;

			diffRAC		[dt]	= diffRac;
			diffClock	[dt]	= -clkCorrection;

			std::cout << std::fixed;
			std::cout << Sat.id() << " " << dt <<" BRatio:       "<< ephRatio << std::endl;
			std::cout << Sat.id() << " " << dt <<" iode:         "<< ssrOut.clkInput.vals[0].iode << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc1:        "<< ssrEphInput.vals[0].brdcPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc2:        "<< ssrEphInput.vals[1].brdcPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec1:        "<< ssrEphInput.vals[0].precPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec2:        "<< ssrEphInput.vals[1].precPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" diffEcef:     "<< posCorrection.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" diffRac:      "<< diffRac.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc1:        "<< ssrClkInput.vals[0].brdcClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc2:        "<< ssrClkInput.vals[1].brdcClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec1:        "<< ssrClkInput.vals[0].precClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec2:        "<< ssrClkInput.vals[1].precClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" clkCorrection:"<< clkCorrection << std::endl;
		}

		ssrEph. deph	= diffRAC[0];
		ssrEph.ddeph	= diffRAC[1] - diffRAC[0];

		ssrClk.dclk[0]	= diffClock[0];
		ssrClk.dclk[1]	= diffClock[1] - diffClock[0];
		ssrClk.dclk[2]	= 0;	// set to zero (not used)

// 		std::cout << "deph:         "<< ssrEph. deph.transpose() << std::endl;
// 		std::cout << "ddeph:        "<< ssrEph.ddeph.transpose() << std::endl;
		std::cout << "deph:         "<< ssrEph. deph.transpose() << std::endl;
		std::cout << Sat.id() << "dclk[0]:        "<< ssrClk.dclk[0] << std::endl;
		std::cout << Sat.id() << "dclk[1]:        "<< ssrClk.dclk[1] << std::endl << std::endl;

#else	/* Bypass interpolation */
		ssrEph.deph     = ecef2rac(ssrEphInput.vals[1].brdcPos, ssrEphInput.vals[1].brdcVel) * posCorrections[1];
		ssrClk.dclk[0]	= -clkCorrections[1];
		ssrClk.dclk[1]	= 0;	// set to zero (not used)
		ssrClk.dclk[2]	= 0;	// set to zero (not used)
#endif

		//adjust all clock corrections so that they remain within the bounds of the outputs
		if (commonClockOffsetsMap[Sat.sys][0] == 0)
		{
			commonClockOffsetsMap[Sat.sys][0] = ssrClk.dclk[0];
			commonClockOffsetsMap[Sat.sys][1] = ssrClk.dclk[1];
			commonClockEpochsMap [Sat.sys][0] = ssrClkInput.vals[0].time;
			commonClockEpochsMap [Sat.sys][1] = ssrClkInput.vals[1].time;
		}
		if	( ssrClkInput.vals[0].time != commonClockEpochsMap[Sat.sys][0]
			||ssrClkInput.vals[1].time != commonClockEpochsMap[Sat.sys][1])
		{
			continue; // commonClockOffset is lagging/leading this sat's clock, skip
		}
		ssrClk.dclk[0] -= commonClockOffsetsMap[Sat.sys][0];
		ssrClk.dclk[1] -= commonClockOffsetsMap[Sat.sys][1];
	}
// 	std::cout << "Calculated Combined Messages.\n";
}

int RtcmEncoder::getUdiIndex(int udi)
{
	for (int i = 0; i < 16; i++) // 16 from updateInterval[16] above
	{
		if (updateInterval[i] == udi)
		{
			return i;
		}
	}

	BOOST_LOG_TRIVIAL(error) << "Error: udi is not valid :" << udi << ").";

	return -1;
}

void RtcmEncoder::encodeWriteMessages(
	std::ostream& outputStream)
{
	if (outputStream)
	{
		outputStream.write((const char*) &data[0], data.size());
		data.clear();
	}
}

bool RtcmEncoder::encodeWriteMessageToBuffer(
	vector<uint8_t>& buffer)
{
	int i = 0;
	int messLength = buffer.size();

	if (buffer.empty())
	{
		return false;
	}

// 	unsigned char nbuf[messLength+6];
	vector<uint8_t> newbuffer(messLength+6);
	unsigned char* nbuf	= newbuffer.data();
	unsigned char* buf	= buffer.data();

	i = setbituInc(nbuf,i,8,	RTCM_PREAMBLE);
	i = setbituInc(nbuf,i,6,	0);
	i = setbituInc(nbuf,i,10,	messLength);

	memcpy(nbuf+3,buf,sizeof(uint8_t)*messLength);
	i = i + messLength*8;

	const unsigned char* bCrcBuf = (const unsigned char *)nbuf;
	unsigned int crcCalc = crc24q(bCrcBuf, sizeof(char)*(messLength+3));

	unsigned char* bCrcCalc = (unsigned char*)&crcCalc;
	unsigned int b1 = 0;
	unsigned int b2 = 0;
	unsigned int b3 = 0;
	setbituInc((unsigned char *)&b1,0,8,*(bCrcCalc+0));
	setbituInc((unsigned char *)&b2,0,8,*(bCrcCalc+1));
	setbituInc((unsigned char *)&b3,0,8,*(bCrcCalc+2));
	i = setbituInc(nbuf,i,8,b3);
	i = setbituInc(nbuf,i,8,b2);
	i = setbituInc(nbuf,i,8,b1);

	data.insert(data.end(), &nbuf[0], &nbuf[messLength+6]);

	return true;
}

vector<uint8_t> RtcmEncoder::encodeTimeStampRTCM()
{
	// Custom message code, for crcsi maximum length 4096 bits or 512 bytes.
	unsigned int messCode = +RtcmMessageType::CUSTOM;
	unsigned int messType = +E_RTCMSubmessage::TIMESTAMP;

	boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();

	// Number of seconds since 1/1/1970, long is 64 bits and all may be used.
	long int seconds = (now - boost::posix_time::from_time_t(0)).total_seconds();

	//Number of fractional seconds, The largest this can be is 1000 which is 10 bits unsigned.
	boost::posix_time::ptime now_mod_seconds	= boost::posix_time::from_time_t(seconds);
	auto subseconds	= now - now_mod_seconds;
	int milli_sec = subseconds.total_milliseconds();

	unsigned int* var = (unsigned int*) &seconds;

	int i = 0;
	//int byteLen = ceil((12.0+8.0+64.0+10.0)/8.0);
	int byteLen = 12;
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();
	i = setbituInc(buf, i, 12,		messCode);
	i = setbituInc(buf, i, 8,		messType);
	i = setbituInc(buf, i, 32,		var[0]);
	i = setbituInc(buf, i, 32,		var[1]);
	i = setbituInc(buf, i, 10, (int)milli_sec);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrComb(
	map<SatSys, SSROut>& ssrOutMap)
{
	int numSat = ssrOutMap.size();
	if (numSat == 0)
	{
		return vector<uint8_t>();
	}

	auto& [Sat, ssrOut1]	= *ssrOutMap.begin();
	auto& ssrEph			= ssrOut1.ssrEph;
	auto& ssrMeta			= ssrEph.ssrMeta;

	int i		= 0;
	int bitLen	= 0;
	int ni		= 0;
	unsigned int messCode = 0;
	if		(Sat.sys == +E_Sys::GPS) {	messCode = +RtcmMessageType::GPS_SSR_COMB_CORR;		ni= 8; bitLen = 68+numSat*205;}
	else if (Sat.sys == +E_Sys::GAL) {	messCode = +RtcmMessageType::GAL_SSR_COMB_CORR;		ni=10; bitLen = 68+numSat*207;}

	int byteLen = ceil(bitLen/8.0);
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	// Write the header information.
	i = setbituInc(buf,i,12,	messCode);
	i = setbituInc(buf,i,20,	ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,		ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,		ssrMeta.multipleMessage);
	i = setbituInc(buf,i,1,		ssrMeta.referenceDatum);
	i = setbituInc(buf,i,4,		ssrEph.iod);
	i = setbituInc(buf,i,16,	ssrMeta.provider);
	i = setbituInc(buf,i,4,		ssrMeta.solution);
	i = setbituInc(buf,i,6,		numSat);

	for (auto& [Sat, ssrOut] : ssrOutMap)
	{
		auto& ssrEph = ssrOut.ssrEph;
		auto& ssrClk = ssrOut.ssrClk;

		i = setbituInc(buf,i, 6,	Sat.prn);
		i = setbituInc(buf,i, ni,	ssrEph.iode);

		int d;
		d = (int)round(ssrEph.deph[0]		/ 0.1e-3);				i = setbitsInc(buf,i,22,d);
		d = (int)round(ssrEph.deph[1]		/ 0.4e-3);				i = setbitsInc(buf,i,20,d);
		d = (int)round(ssrEph.deph[2]		/ 0.4e-3);				i = setbitsInc(buf,i,20,d);
		d = (int)round(ssrEph.ddeph[0]		/ 0.001e-3);			i = setbitsInc(buf,i,21,d);
		d = (int)round(ssrEph.ddeph[1]		/ 0.004e-3);			i = setbitsInc(buf,i,19,d);
		d = (int)round(ssrEph.ddeph[2]		/ 0.004e-3);			i = setbitsInc(buf,i,19,d);

		d = (int)round(ssrClk.dclk[0]		/ 0.1e-3);				i = setbitsInc(buf,i,22,d);
		d = (int)round(ssrClk.dclk[1]		/ 0.001e-3);			i = setbitsInc(buf,i,21,d);
		d = (int)round(ssrClk.dclk[2]		/ 0.00002e-3);			i = setbitsInc(buf,i,27,d);

		outputSsrEphToJson(ssrEph, Sat);
		outputSsrClkToJson(ssrClk, Sat);

	}

	int bitl = byteLen*8-i;
	if (bitl > 7 )
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding combined.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}
	i = setbituInc(buf, i, bitl, 0);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrPhase(
	SsrPBMap& ssrPBMap)
{
	int numSat = ssrPBMap.size();

	int i = 0;
	int bitLen = 0;

	int totalNbias = 0;
	for (auto& [Sat,ssrPhasBias] : ssrPBMap)
	{
		totalNbias += ssrPhasBias.obsCodeBiasMap.size();
	}

	if (totalNbias == 0)
		return vector<uint8_t>();

	// Write the header information.
	auto s_it = ssrPBMap.begin();
	auto& [Sat, ssrPhasBias] = *s_it;
	SSRMeta& ssrMeta = ssrPhasBias.ssrMeta;

	int np = 0;
	int ni = 0;
	int nj = 0;
	int offp = 0;
	unsigned int messCode = 0;
	if (Sat.sys == +E_Sys::GPS) {	messCode = +RtcmMessageType::GPS_SSR_PHASE_BIAS;	np=6; ni= 8; nj= 0; offp=0;	bitLen = 69+numSat*28+totalNbias*32;}
	if (Sat.sys == +E_Sys::GAL)	{	messCode = +RtcmMessageType::GAL_SSR_PHASE_BIAS;	np=6; ni=10; nj= 0; offp=0;	bitLen = 69+numSat*28+totalNbias*32;}

	int byteLen = ceil(bitLen/8.0);
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	i = setbituInc(buf,i,12,messCode);
	i = setbituInc(buf,i,20,ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,	ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,	ssrMeta.multipleMessage);

	i = setbituInc(buf,i,4,	ssrPhasBias.iod);
	i = setbituInc(buf,i,16,ssrMeta.provider);
	i = setbituInc(buf,i,4,	ssrMeta.solution);

	i = setbituInc(buf,i,1,	ssrPhasBias.ssrPhase.dispBiasConistInd);
	i = setbituInc(buf,i,1,	ssrPhasBias.ssrPhase.MWConistInd);

	i = setbituInc(buf,i,6,numSat);

	for (auto& [sat, ssrPhasBias] : ssrPBMap)
	{
		SSRPhase ssrPhase = ssrPhasBias.ssrPhase;

		int d;
															i = setbituInc(buf,i,np,sat.prn);
		d = ssrPhasBias.obsCodeBiasMap.size();				i = setbituInc(buf,i,5,	d);
		d = (int)round(ssrPhase.yawAngle	*256	/PI);	i = setbituInc(buf,i,9,	d);
		d = (int)round(ssrPhase.yawRate		*8192	/PI);	i = setbitsInc(buf,i,8,	d);

		for (auto& [obsCode, entry] : ssrPhasBias.obsCodeBiasMap)
		{
			SSRPhaseCh ssrPhaseCh = ssrPhasBias.ssrPhaseChs[obsCode];

			//BOOST_LOG_TRIVIAL(debug) << "Phase, obsCode : " << obsCode << std::endl;
			//BOOST_LOG_TRIVIAL(debug) << "E_sys         : " << sys << std::endl;
			//BOOST_LOG_TRIVIAL(debug) << "mCodes_gps.size() : " << mCodes_gps.size() << std::endl;
			//print_map( mCodes_gps.left, " E_ObsCode <--> RTCM ", BOOST_LOG_TRIVIAL(debug) );

			int rtcm_code = 0;
			if		( sat.sys == +E_Sys::GPS )		rtcm_code = mCodes_gps.left.at(obsCode);		//todo aaron, crash heaven, needs else, try
			else if ( sat.sys == +E_Sys::GAL )		rtcm_code = mCodes_gal.left.at(obsCode);

			//BOOST_LOG_TRIVIAL(debug) << "rtcm_code      : " << rtcm_code << std::endl;

													i = setbituInc(buf,i,5,	rtcm_code);
													i = setbituInc(buf,i,1,	ssrPhaseCh.signalIntInd);
													i = setbituInc(buf,i,2,	ssrPhaseCh.signalWidIntInd);
													i = setbituInc(buf,i,4,	ssrPhaseCh.signalDisconCnt);
			d = (int)round(entry.bias / 0.0001);	i = setbitsInc(buf,i,20,d);

			traceSsrPhasB(sat, obsCode, ssrPhasBias);
		}
	}

	int bitl = byteLen*8-i;
	if (bitl > 7 )
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding SSR Phase.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}

	i = setbituInc(buf,i,bitl,0);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrCode(
	SsrCBMap& ssrCBMap)
{
	int numSat = ssrCBMap.size();

	int i = 0;

	int totalNbias = 0;
	for (auto s_it = ssrCBMap.begin(); s_it != ssrCBMap.end(); s_it++)
	{
		SSRCodeBias ssrCodeBias = s_it->second;
		totalNbias += ssrCodeBias.obsCodeBiasMap.size();
	}

	if (totalNbias == 0)
	{
		return vector<uint8_t>();
	}

	// Write the header information.

	auto s_it = ssrCBMap.begin();
	auto& [Sat, ssrCodeBias] = *s_it;
	SSRMeta& ssrMeta = ssrCodeBias.ssrMeta;

	int bitLen	= 67
				+ 11 * numSat
				+ 19 * totalNbias;
	int byteLen = ceil(bitLen / 8.0);

	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	unsigned int messCode = 0;
	if (Sat.sys == +E_Sys::GPS)	{	messCode = +RtcmMessageType::GPS_SSR_CODE_BIAS;}
	if (Sat.sys == +E_Sys::GAL)	{	messCode = +RtcmMessageType::GAL_SSR_CODE_BIAS;}

	i = setbituInc(buf,i,12,messCode);
	i = setbituInc(buf,i,20,ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,	ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,	ssrMeta.multipleMessage);

	i = setbituInc(buf,i,4,	ssrCodeBias.iod);
	i = setbituInc(buf,i,16,ssrMeta.provider);
	i = setbituInc(buf,i,4,	ssrMeta.solution);
	i = setbituInc(buf,i,6,	numSat);

	for (auto& [sat, ssrCodeBias] : ssrCBMap)
	{
		i = setbituInc(buf, i, 6, sat.prn);
		unsigned int nbias = ssrCodeBias.obsCodeBiasMap.size();

		i = setbituInc(buf, i, 5, nbias);

		for (auto& [obsCode, entry] : ssrCodeBias.obsCodeBiasMap)
		{
			int rtcm_code = 0;
			if		( sat.sys == +E_Sys::GPS )	{	rtcm_code = mCodes_gps.left.at(obsCode);		}
			else if ( sat.sys == +E_Sys::GAL )	{	rtcm_code = mCodes_gal.left.at(obsCode);		}

													i = setbituInc(buf, i, 5,	rtcm_code);
			int d = (int)round(entry.bias / 0.01);	i = setbitsInc(buf, i, 14,	d);

			traceSsrCodeB(sat, obsCode, ssrCodeBias);
		}
	}

	int bitl = byteLen*8-i;
	if (bitl > 7)
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding SSR Code.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}

	i = setbituInc(buf,i,bitl,0);

	return buffer;
}

max_stars_repo_name: HiTMonitor/ginan

max_issues_repo_name: HiTMonitor/ginan

max_forks_repo_name: HiTMonitor/ginan


text:
#include "../core/wikiDataCache.h"
#include <boost/filesystem.hpp>

#include "../../libs/shared/cpp/stepTimer.hpp"

#include "serverApplication.h"

int main(int argc, char* argv[])
{
	using namespace WikiMainPath;

	auto data_path = boost::filesystem::path(argv[1]);

	if(!boost::filesystem::is_directory(data_path))
	{
		std::cerr << "Specified folder is non-existent." << std::endl;
		return 1;
	}

	std::cout << "Scanning path " << data_path << " for data files." << std::endl;
	Shared::StepTimer timer;

	timer.start_timing_step("global","Reading in data files", &std::cout);

	WikiDataCache data(data_path.string());
	timer.start_timing_step("read_article_titles","Reading in 'article_titles'", &std::cout);
	data.article_titles();
	timer.stop_timing_step("read_article_titles",&std::cout);
	timer.start_timing_step("read_category_titles","Reading in 'category_titles'", &std::cout);
	data.category_titles();
	timer.stop_timing_step("read_category_titles",&std::cout);
	timer.start_timing_step("read_category_has_article","Reading in 'category_has_article'", &std::cout);
	data.category_has_article();
	timer.stop_timing_step("read_category_has_article",&std::cout);
	timer.start_timing_step("read_article_dates","Reading in 'article_dates'", &std::cout);
	data.article_dates();
	timer.stop_timing_step("read_article_dates",&std::cout);
	timer.start_timing_step("read_article_network","Reading in 'article_network'", &std::cout);
	data.article_network();
	timer.stop_timing_step("read_article_network",&std::cout);
	timer.start_timing_step("read_category_hirachy_graph","Reading in 'category_hirachy_graph'", &std::cout);
	data.category_hirachy_graph();
	timer.stop_timing_step("read_category_hirachy_graph",&std::cout);
	// timer.start_timing_step("read_event_network","Reading in 'event_network'", &std::cout);
	// data.event_network();
	// timer.stop_timing_step("read_event_network",&std::cout);
	// timer.start_timing_step("read_event_indices","Reading in 'event_indices'", &std::cout);
	// data.event_indices();
	// timer.stop_timing_step("read_event_indices",&std::cout);

	std::cout << "---------------------------------------------------" << std::endl;
	std::cout << "IO Timings: " << std::endl;
	timer.print_timings(std::cout);
	std::cout << "---------------------------------------------------" << std::endl;

	Shared::StepTimer timer_server;

	timer_server.start_timing_step("server_running","Server running", &std::cout);
	Server server(data);
	server.run(argc,argv);
	timer_server.stop_timing_step("server_running", &std::cout);

	std::cout << "---------------------------------------------------" << std::endl;
	std::cout << "Running Timings: " << std::endl;
	timer_server.print_timings(std::cout);
	std::cout << "---------------------------------------------------" << std::endl;

	return 0;
}

max_stars_repo_name: bencabrera/wikiMainPath

max_issues_repo_name: bencabrera/wikiMainPath

max_forks_repo_name: bencabrera/wikiMainPath


text:
//=======================================================================
// Copyright 2008
// Author: Matyas W Egyhazy
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_METRIC_TSP_APPROX_HPP
#define BOOST_GRAPH_METRIC_TSP_APPROX_HPP

// metric_tsp_approx
// Generates an approximate tour solution for the traveling salesperson
// problem in polynomial time. The current algorithm guarantees a tour with a
// length at most as long as 2x optimal solution. The graph should have
// 'natural' (metric) weights such that the triangle inequality is maintained.
// Graphs must be fully interconnected.

// TODO:
// There are a couple of improvements that could be made.
// 1) Change implementation to lower uppper bound Christofides heuristic
// 2) Implement a less restrictive TSP heuristic (one that does not rely on
//    triangle inequality).
// 3) Determine if the algorithm can be implemented without creating a new
//    graph.

#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/concept_check.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_as_tree.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/throw_exception.hpp>

namespace boost
{
    // Define a concept for the concept-checking library.
    template <typename Visitor, typename Graph>
    struct TSPVertexVisitorConcept
    {
    private:
        Visitor vis_;
    public:
        typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

        BOOST_CONCEPT_USAGE(TSPVertexVisitorConcept)
        {
            Visitor vis(vis_);  // require copy construction
            Graph g(1);
            Vertex v(*vertices(g).first);
            vis_.visit_vertex(v, g); // require visit_vertex
        }
    };

    // Tree visitor that keeps track of a preorder traversal of a tree
    // TODO: Consider migrating this to the graph_as_tree header.
    // TODO: Parameterize the underlying stores o it doesn't have to be a vector.
    template<typename Node, typename Tree> class PreorderTraverser
    {
    private:
        std::vector<Node>& path_;
    public:
        typedef typename std::vector<Node>::const_iterator const_iterator;

        PreorderTraverser(std::vector<Node>& p) : path_(p) {}

        void preorder(Node n, const Tree&)
        { path_.push_back(n); }

        void inorder(Node, const Tree&) const {}
        void postorder(Node, const Tree&) const {}

        const_iterator begin() const { return path_.begin(); }
        const_iterator end() const { return path_.end(); }
    };

    // Forward declarations
    template <typename> class tsp_tour_visitor;
    template <typename, typename, typename, typename> class tsp_tour_len_visitor;

    template<typename VertexListGraph, typename OutputIterator>
    void metric_tsp_approx_tour(VertexListGraph& g, OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first,
            get(edge_weight, g), get(vertex_index, g),
            tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename WeightMap, typename OutputIterator>
    void metric_tsp_approx_tour(VertexListGraph& g, WeightMap w, OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first,
            w, tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename OutputIterator>
    void metric_tsp_approx_tour_from_vertex(VertexListGraph& g,
        typename graph_traits<VertexListGraph>::vertex_descriptor start,
        OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, start, get(edge_weight, g),
            get(vertex_index, g), tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename WeightMap,
        typename OutputIterator>
    void metric_tsp_approx_tour_from_vertex(VertexListGraph& g,
    typename graph_traits<VertexListGraph>::vertex_descriptor start,
        WeightMap w, OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, start, w, get(vertex_index, g),
            tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename TSPVertexVisitor>
    void metric_tsp_approx(VertexListGraph& g, TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first,
            get(edge_weight, g), get(vertex_index, g), vis);
    }

    template<typename VertexListGraph, typename Weightmap,
        typename VertexIndexMap, typename TSPVertexVisitor>
    void metric_tsp_approx(VertexListGraph& g, Weightmap w,
        TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first, w,
            get(vertex_index, g), vis);
    }

    template<typename VertexListGraph, typename WeightMap,
        typename VertexIndexMap, typename TSPVertexVisitor>
    void metric_tsp_approx(VertexListGraph& g, WeightMap w, VertexIndexMap id,
        TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first, w, id, vis);
    }

    template<typename VertexListGraph, typename WeightMap,
        typename TSPVertexVisitor>
    void metric_tsp_approx_from_vertex(VertexListGraph& g,
    typename graph_traits<VertexListGraph>::vertex_descriptor start,
        WeightMap w, TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, start, w, get(vertex_index, g), vis);
    }

    template <
        typename VertexListGraph,
        typename WeightMap,
        typename VertexIndexMap,
        typename TSPVertexVisitor>
    void metric_tsp_approx_from_vertex(const VertexListGraph& g,
                                       typename graph_traits<VertexListGraph>::vertex_descriptor start,
                                       WeightMap weightmap,
                                       VertexIndexMap indexmap,
                                       TSPVertexVisitor vis)
    {
        using namespace boost;
        using namespace std;

        BOOST_CONCEPT_ASSERT((VertexListGraphConcept<VertexListGraph>));
        BOOST_CONCEPT_ASSERT((TSPVertexVisitorConcept<TSPVertexVisitor, VertexListGraph>));

        // Types related to the input graph (GVertex is a template parameter).
        typedef typename graph_traits<VertexListGraph>::vertex_descriptor GVertex;
        typedef typename graph_traits<VertexListGraph>::vertex_iterator GVItr;

        // We build a custom graph in this algorithm.
        typedef adjacency_list <vecS, vecS, directedS, no_property, no_property > MSTImpl;
        typedef graph_traits<MSTImpl>::vertex_descriptor Vertex;
        typedef graph_traits<MSTImpl>::vertex_iterator VItr;

        // And then re-cast it as a tree.
        typedef iterator_property_map<vector<Vertex>::iterator, property_map<MSTImpl, vertex_index_t>::type> ParentMap;
        typedef graph_as_tree<MSTImpl, ParentMap> Tree;
        typedef tree_traits<Tree>::node_descriptor Node;

        // A predecessor map.
        typedef vector<GVertex> PredMap;
        typedef iterator_property_map<typename PredMap::iterator, VertexIndexMap> PredPMap;

        PredMap preds(num_vertices(g));
        PredPMap pred_pmap(preds.begin(), indexmap);

        // Compute a spanning tree over the in put g.
        prim_minimum_spanning_tree(g, pred_pmap,
             root_vertex(start)
            .vertex_index_map(indexmap)
            .weight_map(weightmap));

        // Build a MST using the predecessor map from prim mst
        MSTImpl mst(num_vertices(g));
        std::size_t cnt = 0;
        pair<VItr, VItr> mst_verts(vertices(mst));
        for(typename PredMap::iterator vi(preds.begin()); vi != preds.end(); ++vi, ++cnt)
        {
            if(indexmap[*vi] != cnt) {
                add_edge(*next(mst_verts.first, indexmap[*vi]),
                         *next(mst_verts.first, cnt), mst);
            }
        }

        // Build a tree abstraction over the MST.
        vector<Vertex> parent(num_vertices(mst));
        Tree t(mst, *vertices(mst).first,
            make_iterator_property_map(parent.begin(),
            get(vertex_index, mst)));

        // Create tour using a preorder traversal of the mst
        vector<Node> tour;
        PreorderTraverser<Node, Tree> tvis(tour);
        traverse_tree(indexmap[start], t, tvis);

        pair<GVItr, GVItr> g_verts(vertices(g));
        for(PreorderTraverser<Node, Tree>::const_iterator curr(tvis.begin());
            curr != tvis.end(); ++curr)
        {
            // TODO: This is will be O(n^2) if vertex storage of g != vecS.
            GVertex v = *next(g_verts.first, get(vertex_index, mst)[*curr]);
            vis.visit_vertex(v, g);
        }

        // Connect back to the start of the tour
        vis.visit_vertex(start, g);
    }

    // Default tsp tour visitor that puts the tour in an OutputIterator
    template <typename OutItr>
    class tsp_tour_visitor
    {
        OutItr itr_;
    public:
        tsp_tour_visitor(OutItr itr)
            : itr_(itr)
        { }

        template <typename Vertex, typename Graph>
        void visit_vertex(Vertex v, const Graph&)
        {
            BOOST_CONCEPT_ASSERT((OutputIterator<OutItr, Vertex>));
            *itr_++ = v;
        }

    };

    // Tsp tour visitor that adds the total tour length.
    template<typename Graph, typename WeightMap, typename OutIter, typename Length>
    class tsp_tour_len_visitor
    {
        typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
        BOOST_CONCEPT_ASSERT((OutputIterator<OutIter, Vertex>));

        OutIter iter_;
        Length& tourlen_;
        WeightMap& wmap_;
        Vertex previous_;

        // Helper function for getting the null vertex.
        Vertex null()
        { return graph_traits<Graph>::null_vertex(); }

    public:
        tsp_tour_len_visitor(Graph const&, OutIter iter, Length& l, WeightMap map)
            : iter_(iter), tourlen_(l), wmap_(map), previous_(null())
        { }

        void visit_vertex(Vertex v, const Graph& g)
        {
            typedef typename graph_traits<Graph>::edge_descriptor Edge;

            // If it is not the start, then there is a
            // previous vertex
            if(previous_ != null())
            {
                // NOTE: For non-adjacency matrix graphs g, this bit of code
                // will be linear in the degree of previous_ or v. A better
                // solution would be to visit edges of the graph, but that
                // would require revisiting the core algorithm.
                Edge e;
                bool found;
                boost::tie(e, found) = lookup_edge(previous_, v, g);
                if(!found) {
                    BOOST_THROW_EXCEPTION(not_complete());
                }

                tourlen_ += wmap_[e];
            }

            previous_ = v;
            *iter_++ = v;
        }
    };

    // Object generator(s)
    template <typename OutIter>
    inline tsp_tour_visitor<OutIter>
    make_tsp_tour_visitor(OutIter iter)
    { return tsp_tour_visitor<OutIter>(iter); }

    template <typename Graph, typename WeightMap, typename OutIter, typename Length>
    inline tsp_tour_len_visitor<Graph, WeightMap, OutIter, Length>
    make_tsp_tour_len_visitor(Graph const& g, OutIter iter, Length& l, WeightMap map)
    { return tsp_tour_len_visitor<Graph, WeightMap, OutIter, Length>(g, iter, l, map); }

} //boost

#endif // BOOST_GRAPH_METRIC_TSP_APPROX_HPP

max_stars_repo_name: multi-os-engine/cinder-natj-binding

max_issues_repo_name: multi-os-engine/cinder-natj-binding

max_forks_repo_name: multi-os-engine/cinder-natj-binding