sttl.cpp

#include "sttl.h"

#include <iostream>

#include <json/json.h>
#include <json/value.h>

using namespace std;

void testSTTL(){
	cout<<"Hello STTL"<<endl;

}




Eigen::MatrixXd ReadJsonMatrix(Json::Value Array){
    int rows = Array.size();
    int cols = Array[0].size();

    Eigen::MatrixXd Matrix(rows,cols) ;

    for(int i = 0 ; i<rows ; i++){
        for(int j=0 ; j<cols ; j++){
            Matrix(i,j) = Array[i][j].asDouble() ;
        }
    }
    return Matrix;
}

//// Read Json Array and return as Eigen Vector
Eigen::VectorXd ReadJsonVector(Json::Value Array){
    int size = Array.size();

    Eigen::VectorXd Vector(size);
    for(int i = 0 ; i < size ; i++){
        Vector(i) = Array[i].asDouble();
    }
    return Vector;
}



bool fillJson(const sttl::Frame & frame, Json::Value & node){
	node["name"] = frame.name;


	for(int i=0; i<4; ++i){
		for(int j=0; j<4; ++j){

		}
	}
	Json::Value trans(Json::arrayValue);
	trans.resize(4);
		for(int i=0; i<4; ++i){
			trans[i]=Json::Value(Json::arrayValue);
			trans[i].resize(4);
			for(int k=0; k<4; ++k){
				trans[i][k] = frame.transform(i, k);
			}
		}
	node["transform"] = trans;



	if(frame.hasCovariance){
	Json::Value cov(Json::arrayValue);
	cov.resize(7);
		for(int i=0; i<7; ++i){
			cov[i]=Json::Value(Json::arrayValue);
			cov[i].resize(7);
			for(int k=0; k<7; ++k){
				cov[i][k] = frame.covariance(i, k);
			}
		}
	node["covariance"] = cov;
	}

	node["referenceFrame"] = frame.referenceFrameName;

	//std::cout<<node; 
	return true;
}


bool sttl::saveList(const std::list<sttl::Frame> & frames){
	Json::Value root(Json::arrayValue);
	for(std::list<sttl::Frame>::const_iterator itr = frames.begin(); itr != frames.end(); ++itr){
		Json::Value frame;
		fillJson(*itr, frame);
		root.append(frame);
	}

	std::cout<<root;


	return true;
}