<commit_before>///////////////////////////////////////////////////////////////////////////////
//
//  Copyright (c) 2007, mjackson
//  All rights reserved.
//  BSD License: http://www.opensource.org/licenses/bsd-license.html
//
//  This code was written under United States Air Force Contract number
//                           FA8650-04-C-5229
//
///////////////////////////////////////////////////////////////////////////////

//-- MXA Includes
#include <MXAConfiguration.h>
#include <TestDataFileLocations.h>
#include <Common/MXATypeDefs.h>
#include <Base/IDataFile.h>
#include <Base/IDataModel.h>
#include <Base/IFileWriter.h>
#include <Core/MXADataModel.h>
#include <HDF5/H5MXADataFile.h>
#include <HDF5/H5Utilities.h>

#include <Dataset/IAbstractDataset.h>
#include <Dataset/IAbstractAttribute.h>
#include <Dataset/H5StringDataset.h>
#include <Dataset/H5StringAttribute.h>
#include <Dataset/H5PointerDataset.h>
#include <Dataset/H5PointerAttribute.h>
#include <Dataset/H5VectorRefDataset.h>

// C++ Includes
#include <iostream>
#include <string>

//-- Boost Test Headers
#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>
//-- Boost Filesystem Headers
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/convenience.hpp>

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
#define IFILEWRITER_DYNAMIC_CAST(inVar, outvar)   \
  boost::shared_ptr<IFileWriter> outvar = boost::dynamic_pointer_cast<IFileWriter>(inVar);

#define SHARED_POINTER_DYNAMIC_CAST(fromVar, toType, toVar) \
  boost::shared_ptr<toType> toVar = boost::dynamic_pointer_cast<toType>(fromVar);

#define DECLARE_REFERENCE_VARIABLE(classType ,shared_ptr, ref) \
  classType &ref = *(shared_ptr.get());

#define CreatePointerAttribute(U, dsPath, key) \
  name.clear(); \
  name.append(key).append(" attribute");\
  U U##vec[4];\
  for (int i = 0; i < 4; ++i) { U##vec[i] = (U)(127); } \
  attr = H5PointerAttribute<U>::New(dsPath, name, attrDimensions, U##vec );\
  testDataset->addAttribute(attr);

#define CreateVectorAttribute(U, dsPath, key) \
name.clear(); \
name.append(key).append(" attribute");\
U U##vec[4];\
for (int i = 0; i < 4; ++i) { U##vec[i] = (U)(127); } \
attr = H5PointerAttribute<U>::New(dsPath, name, attrDimensions, U##vec );\
testDataset->addAttribute(attr);

// Declare all the methods
IDataModelPtr createModel();
void DatasetTest();
void MakeDataRecord( const std::string &key, IDataModelPtr model);

int recLuid = 0;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void RemoveTestFiles()
{
#if REMOVE_TEST_FILES
  BOOST_REQUIRE ( boost::filesystem::remove(DATASET_TEST_FILE) == true );
#endif
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------

void MakeDataRecord( const std::string &key, IDataModelPtr model)
{
  MXADataRecordPtr rec = MXADataRecord::New(recLuid++, key, key );
  model->addDataRecord(rec);
}

// -----------------------------------------------------------------------------
//  Creates a Data Model to use
// -----------------------------------------------------------------------------
IDataModelPtr createModel()
{
    std::string errorMessage;
    IDataModelPtr model = MXADataModel::New();

    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == false );

    model->setDataRoot(std::string("DatasetTest/"));
    errorMessage.clear();
    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == false );
    model->setModelType(MXA::MXACurrentFileType);
    errorMessage.clear();
    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == false );
    model->setModelVersion(MXA::MXACurrentFileVersion);
    errorMessage.clear();
    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == false );

    // ---------- Test creation/addition of Data Dimensions
    IDataDimensionPtr dim0 = model->addDataDimension("Data Container", "Data Container",  10, 0, 9, 1, 1);
    errorMessage.clear();
    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == false );

    //Create Data Records for the Pointer Tests
    MakeDataRecord( "Pointer Int 8", model);
    MakeDataRecord( "Pointer UInt8", model);
    MakeDataRecord( "Pointer Int 16", model);
    MakeDataRecord( "Pointer UInt16", model);
    MakeDataRecord( "Pointer Int 32", model);
    MakeDataRecord( "Pointer UInt32", model);
    MakeDataRecord( "Pointer Int 64", model);
    MakeDataRecord( "Pointer UInt64", model);
    // Floating point Numbers
    MakeDataRecord( "Pointer Float 32", model);
    MakeDataRecord( "Pointer Float 64", model);

    // Create Data record for the String test
    MakeDataRecord( "String", model);

    // Create Data Records for the Vector Tests
    MakeDataRecord( "Vector Int 8", model);
    MakeDataRecord( "Vector UInt8", model);
    MakeDataRecord( "Vector Int 16", model);
    MakeDataRecord( "Vector UInt16", model);
    MakeDataRecord( "Vector Int 32", model);
    MakeDataRecord( "Vector UInt32", model);
    MakeDataRecord( "Vector Int 64", model);
    MakeDataRecord( "Vector UInt64", model);
    // Floating point Numbers
    MakeDataRecord( "Vector Float 32", model);
    MakeDataRecord( "Vector Float 64", model);

    errorMessage.clear();
    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == false );

    //Create the Required MetaData
    std::map<std::string, std::string> md;
    md[MXA::MXA_CREATOR_TAG] = "Mike Jackson";
    md[MXA::MXA_DATE_TAG] = "2007:12:24 15:34.51";
    md[MXA::MXA_DSET_NAME_TAG] = "TESTING Example Data Model";
    md[MXA::MXA_DESCRIPTION_TAG] = "Testing the Data Containers API";
    md[MXA::MXA_PEDIGREE_TAG] = "Original";
    md[MXA::MXA_DERIVED_SRC_TAG] = "Original Data Files";
    md[MXA::MXA_RIGHTS_TAG] = "Unlimited";
    md[MXA::MXA_RELEASE_NUMBER_TAG] = "Not Applicable";

    int32 err = -1;
    err = model->setRequiredMetaData(md);
    errorMessage.clear();
    BOOST_REQUIRE ( (model->isValid(errorMessage) ) == true );
    return model;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
template<typename T>
int32 PointerTest(T theType, const std::string &recName, IDataFilePtr dataFile)
{
  IDataModelPtr model = dataFile->getDataModel();
  IDataRecordPtr rec = model->getDataRecordByNamedPath(recName, NULL);
  BOOST_REQUIRE(rec.get() != NULL);

  std::vector<int32> mxaDims;
  mxaDims.push_back(2); // This data set is for index '2' of the 'Data Container' MXA Data Dimension
  std::string dsPath = H5Utilities::generateH5PathToDataset(model, mxaDims, rec);

  std::vector<uint64> datasetDims(1, 10); // 1 element with a value of 10
  T dvec[10];
  for (int i = 0; i < 10; ++i)
  {
    dvec[i] = (T)(i * 5643.234523);
  } // Fill with some data
  IAbstractDatasetPtr testDataset = H5PointerDataset<T>::New(dsPath, datasetDims, dvec);
  BOOST_REQUIRE(1 == testDataset->getDatasetDimensions().size() );
  BOOST_REQUIRE(10 == testDataset->getDatasetDimensions().at(0) );

  //Create a string Attribute
  IAbstractAttributePtr strAttribute = H5StringAttribute::New(dsPath, "String Attribute", "Test String");
  BOOST_REQUIRE (strAttribute.get() != NULL);
  testDataset->addAttribute(strAttribute);

  // Create a Pointer based Attribute
  IAbstractAttributePtr attr;
  std::string name;
  std::vector<uint64> attrDimensions(1, 4); // 1 element with a value of 10

  CreatePointerAttribute(int8, dsPath, "Int 8")
  CreatePointerAttribute(uint8, dsPath, "UInt8")
  CreatePointerAttribute(int16, dsPath, "Int 16")
  CreatePointerAttribute(uint16, dsPath, "UInt16")
  CreatePointerAttribute(int32, dsPath, "Int 32")
  CreatePointerAttribute(uint32, dsPath, "UInt32")
  CreatePointerAttribute(int64, dsPath, "Int 64")
  CreatePointerAttribute(uint64, dsPath, "UInt64")
  //  //Floating Point Numbers
  CreatePointerAttribute(float32, dsPath, "Float 32")
  CreatePointerAttribute(float64, dsPath, "Float 64")

  int32 err = dataFile->writeData(testDataset);

  int numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 11);
  err = testDataset->removeAttribute(attr);
  BOOST_REQUIRE (err >= 0);
  numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 10);
  err = testDataset->removeAttribute(9); // Remove the last attribute
  BOOST_REQUIRE (err >= 0);
  numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 9);

  // try removing a value outside the range of the attributes
  err = testDataset->removeAttribute(9);
  BOOST_REQUIRE (err == 0);
  numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 9);

  err = testDataset->removeAttribute(-1);
  BOOST_REQUIRE (err == 0);
  numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 9);

  // Try removing the first attribute at index 0
  err = testDataset->removeAttribute(0);
  BOOST_REQUIRE (err >= 0);
  numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 8);

  // Try removing an attribute from the middle
  err = testDataset->removeAttribute(5);
  BOOST_REQUIRE (err >= 0);
  numAttrs = testDataset->getNumberOfAttributes();
  BOOST_REQUIRE(numAttrs == 7);
  return err;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void H5StringDatasetTest(IDataFilePtr dataFile)
{
   IDataModelPtr model = dataFile->getDataModel();
   IDataRecordPtr rec = model->getDataRecordByNamedPath("String", NULL);
   BOOST_REQUIRE(rec.get() != NULL);

   std::vector<int32> mxaDims;
   mxaDims.push_back(2); // This data set is for index '2' of the 'Data Container' MXA Data Dimension
   std::string data ("This is the string Dataset");
   std::string dsPath = H5Utilities::generateH5PathToDataset(model, mxaDims, rec);

   // Create the dataset
   IAbstractDatasetPtr testDataset = H5StringDataset::New(dsPath, data);
   BOOST_REQUIRE (testDataset.get() != NULL);

  //Create a string Attribute
   IAbstractAttributePtr strAttribute = H5StringAttribute::New(dsPath, "String Attribute", "Test String");
   BOOST_REQUIRE (strAttribute.get() != NULL);
   testDataset->addAttribute(strAttribute);

   // Create a Pointer based Attribute
   IAbstractAttributePtr attr;
   std::string name;
   std::vector<uint64> attrDimensions(1, 4); // 1 element with a value of 10

   CreatePointerAttribute(int8, dsPath, "Int 8")
   CreatePointerAttribute(uint8, dsPath, "UInt8")
   CreatePointerAttribute(int16, dsPath, "Int 16")
   CreatePointerAttribute(uint16, dsPath, "UInt16")
   CreatePointerAttribute(int32, dsPath, "Int 32")
   CreatePointerAttribute(uint32, dsPath, "UInt32")
   CreatePointerAttribute(int64, dsPath, "Int 64")
   CreatePointerAttribute(uint64, dsPath, "UInt64")
   //  //Floating Point Numbers
   CreatePointerAttribute(float32, dsPath, "Float 32")
   CreatePointerAttribute(float64, dsPath, "Float 64")

   int32 err = dataFile->writeData(testDataset);

   int numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 11);
   err = testDataset->removeAttribute(attr);
   BOOST_REQUIRE (err >= 0);
   numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 10);
   err = testDataset->removeAttribute(9); // Remove the last attribute
   BOOST_REQUIRE (err >= 0);
   numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 9);

   // try removing a value outside the range of the attributes
   err = testDataset->removeAttribute(9);
   BOOST_REQUIRE (err == 0);
   numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 9);

   err = testDataset->removeAttribute(-1);
   BOOST_REQUIRE (err == 0);
   numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 9);

   // Try removing the first attribute at index 0
   err = testDataset->removeAttribute(0);
   BOOST_REQUIRE (err >= 0);
   numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 8);

   // Try removing an attribute from the middle
   err = testDataset->removeAttribute(5);
   BOOST_REQUIRE (err >= 0);
   numAttrs = testDataset->getNumberOfAttributes();
   BOOST_REQUIRE(numAttrs == 7);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
template<typename T>
int32 VectorTest(T theType, const std::string &recName, IDataFilePtr dataFile)
{
  IDataModelPtr model = dataFile->getDataModel();
  IDataRecordPtr rec = model->getDataRecordByNamedPath(recName, NULL);
  BOOST_REQUIRE(rec.get() != NULL);

  std::vector<int32> mxaDims;
  mxaDims.push_back(2); // This data set is for index '2' of the 'Data Container' MXA Data Dimension
  std::string dsPath = H5Utilities::generateH5PathToDataset(model, mxaDims, rec);
  // Watch out for Scope issues with feeding H5VectorDataset the data variable. If
  //  'data' were to go out of scope BEFORE the write operation then junk
  //  is going to be written to the file
  std::vector<uint64> datasetDims(1, 10); // 1 element with a value of 10
  std::vector<T> data(10, 0);
  for (int i = 0; i < 10; ++i) { data[i] = (T)(100); }
  IAbstractDatasetPtr testDataset = H5VectorRefDataset<T>::New(dsPath, datasetDims, data);

  int32 err = dataFile->writeData(testDataset);

  return err;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DatasetTest()
{
  std::cout << "Running Dataset Test ....";
  // Create our Test File to output our test data into
  std::string testFile (DATASET_TEST_FILE);
  IDataModelPtr model = createModel();
  IDataFilePtr dataFile = H5MXADataFile::CreateFileWithModel(testFile, model);

  // Test String datasets
  H5StringDatasetTest(dataFile);

  int8  i8  = -8;
  uint8 ui8 = 8;
  int16 i16 = -16;
  uint16 ui16 = 16;
  int32 i32 = -32;
  uint32 ui32 = 32;
  int64 i64 = -64;
  uint64 ui64 = 64;
  float32 f32 = 32.32f;
  float64 f64 = 64.64;

  // Test the Pointer Datasets
  BOOST_REQUIRE ( PointerTest( i8, "Pointer Int 8", dataFile) >= 0 );
  BOOST_REQUIRE ( PointerTest( ui8, "Pointer UInt8", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( i16, "Pointer Int 16", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( ui16, "Pointer UInt16", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( i32, "Pointer Int 32", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( ui32, "Pointer UInt32", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( i64, "Pointer Int 64", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( ui64, "Pointer UInt64", dataFile) >= 0);
  // Floating point Numbers
  BOOST_REQUIRE ( PointerTest( f32, "Pointer Float 32", dataFile) >= 0);
  BOOST_REQUIRE ( PointerTest( f64, "Pointer Float 64", dataFile) >= 0);

  // Test the Vector based dataset classes
  BOOST_REQUIRE ( VectorTest( i8, "Vector Int 8", dataFile) >= 0 );
  BOOST_REQUIRE ( VectorTest( ui8, "Vector UInt8", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( i16, "Vector Int 16", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( ui16, "Vector UInt16", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( i32, "Vector Int 32", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( ui32, "Vector UInt32", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( i64, "Vector Int 64", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( ui64, "Vector UInt64", dataFile) >= 0);
  // Floating point Numbers
  BOOST_REQUIRE ( VectorTest( f32, "Vector Float 32", dataFile) >= 0);
  BOOST_REQUIRE ( VectorTest( f64, "Vector Float 64", dataFile) >= 0);

  std::cout << "... Passed." << std::endl;
}

// -----------------------------------------------------------------------------
//  Use Boost unit test framework
// -----------------------------------------------------------------------------
boost::unit_test::test_suite* init_unit_test_suite( int32 /*argc*/, char* /*argv*/[] )
{
  boost::unit_test::test_suite* test= BOOST_TEST_SUITE( "Dataset Tests" );
  test->add( BOOST_TEST_CASE( &DatasetTest), 0);
  test->add( BOOST_TEST_CASE( &RemoveTestFiles), 0);
  return test;
}

<commit_msg>Removing file that should not have been committed to CVS.<commit_after><|endoftext|>


<commit_before>#include <ros/ros.h>

#include <math.h>
#include <stdint.h>
#include <stdbool.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <opencv2/highgui/highgui.hpp>
#include <quirkd/Alert.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

class DataController {
    public:
        DataController()
            : it_(n_)
        {
            alert_pub_ = n_.advertise<quirkd::Alert>("/quirkd/alert/notification", 1);
            laser_sub_ = n_.subscribe("/base_scan", 1, &DataController::laserScanCB, this);
            ROS_INFO("WaitForService(\"static_map\");");
            ros::service::waitForService("static_map");
            static_map_client_ = n_.serviceClient<nav_msgs::GetMap>("static_map");
            ROS_INFO("WaitForService(\"dynamic_map\");");
            ros::service::waitForService("dynamic_map");
            dynamic_map_client_ = n_.serviceClient<nav_msgs::GetMap>("dynamic_map");
            static_image_pub_ = it_.advertise("/quirkd/test/static_image", 1);
            dynamic_image_pub_ = it_.advertise("/quirkd/test/dynamic_image", 1);
            heatmap_pub_ = it_.advertise("/quirkd/test/heatmap", 1);
        }
        void laserScanCB(const sensor_msgs::LaserScan msg) {
            ROS_INFO("Laser Scan Callback");
            updated = true;
            last_data = msg;
            try {
                tf_.lookupTransform("/map", "/base_laser_link", ros::Time(0), last_tf);
                ROS_INFO("tf success for /map to /base_laser_link");
            } catch (tf::TransformException &ex) {
                ROS_WARN("tf fetch failed. %s", ex.what());
            }
        }
        void update() {
            quirkd::Alert alert;
            alert.level = 0;
            alert.min_x = 0;
            alert.max_x = 1;
            alert.min_y = 0;
            alert.max_y = 1;
            updateAlertPerimeter(&alert, last_data, last_tf);

            ROS_INFO("Update Data Processor");
            nav_msgs::GetMap srv;
            cv_bridge::CvImagePtr static_image;
            cv_bridge::CvImagePtr dynamic_image;
            if (static_map_client_.call(srv)) {
                ROS_INFO("Successfull call static map");
                nav_msgs::OccupancyGrid og = srv.response.map;
                static_image = this->gridToCroppedCvImage(&og, &alert);

                static_image_pub_.publish(static_image->toImageMsg());

            } else {
                ROS_WARN("Failed to get static map");
            }
            if (dynamic_map_client_.call(srv)) {
                ROS_INFO("Successfull call dynamic map");
                nav_msgs::OccupancyGrid og = srv.response.map;
                dynamic_image = this->gridToCroppedCvImage(&og, &alert);

                dynamic_image_pub_.publish(dynamic_image->toImageMsg());
            } else {
                ROS_WARN("Failed to get dynamic map");
                dynamic_image = static_image;
            }

            // two images (static (base) image and dynamic image)
            // perimeter encoded as part of the alert and images aligned to alert perimeter

            /* Compute difference
             * Steps:
             * - manhattan distance
             */

            // Manhattan Distance
            cv::Mat absdiff;
            cv::absdiff(static_image->image, dynamic_image->image, absdiff);

            // use absdiff as a mask to visualize difference with "heatmap" (bgr)
            cv::Mat redBase(static_image->image.rows, static_image->image.cols, CV_8UC3, cv::Scalar(0, 0, 255));
            cv::Mat byChannel[3];
            cv::split(redBase, byChannel);
            // byChannel[2] = absdiff;
            cv::merge(byChannel, 3, redBase);
            ROS_INFO("Split and merge done");
            cv_bridge::CvImagePtr heatmap;
            ROS_INFO("Create heatmap");
            // problem in this step
            heatmap->image = redBase;
            ROS_INFO("Add redBase");
            heatmap_pub_.publish(heatmap->toImageMsg());
            ROS_INFO("publish heatmap");

            alert.level = cv::sum(absdiff)[0];

            // TODO publish difference
            ROS_INFO("The two images have a difference of %d", alert.level);
            alert_pub_.publish(alert);

            updated = false;
        }
        bool updated;
        sensor_msgs::LaserScan last_data;
        tf::StampedTransform last_tf;
    private:
        ros::NodeHandle n_;
        image_transport::ImageTransport it_;
        image_transport::Publisher static_image_pub_;
        image_transport::Publisher dynamic_image_pub_;
        image_transport::Publisher heatmap_pub_;
        ros::Publisher alert_pub_;
        ros::Subscriber laser_sub_;
        ros::ServiceClient dynamic_map_client_;
        ros::ServiceClient static_map_client_;
        tf::TransformListener tf_;
        void updateAlertPerimeter(quirkd::Alert* alert, const sensor_msgs::LaserScan scan, const tf::StampedTransform tf) {
            double base_x = tf.getOrigin().x();
            double base_y = tf.getOrigin().y();
            double r, p, base_heading;
            tf::Matrix3x3 m(tf.getRotation());
            m.getRPY(r, p, base_heading);

            double scan_min = base_heading + scan.angle_min;
            double scan_inc = scan.angle_increment;
            double heading = scan_min;

            alert->min_x = base_x;
            alert->max_x = base_x;
            alert->min_y = base_y;
            alert->max_y = base_y;

            double live_x, live_y;

            for (int i = 0; i < scan.ranges.size(); i++) {
                double dist = scan.ranges[i];

                live_x = base_x + dist * cos(heading);
                live_y = base_y + dist * sin(heading);
                alert->min_x = std::min(live_x, double(alert->min_x));
                alert->max_x = std::max(live_x, double(alert->max_x));
                alert->min_y = std::min(live_y, double(alert->min_y));
                alert->max_y = std::max(live_y, double(alert->max_y));

                heading += scan_inc;
            }
            double padding = 0.5;
            alert->min_x += -padding;
            alert->min_y += -padding;
            alert->max_x += padding;
            alert->max_y += padding;
        }
        cv_bridge::CvImagePtr gridToCroppedCvImage(nav_msgs::OccupancyGrid* grid, quirkd::Alert* alert) {
            // Unpack the Occupancy Grid

            nav_msgs::MapMetaData info = grid->info;
            float resolution = info.resolution; // meters per pixel
            int cell_width = info.width;
            float map_width = cell_width * resolution;
            int cell_height = info.height;
            float map_height = cell_height * resolution;
            // I'm assuming the map isn't rotated right now because that could be really complex
            float origin_x = info.origin.position.x;
            float origin_y = info.origin.position.x;
            /*
             * From documentation:
             * The map data in row major order, starting at 0,0.
             * Valid values are in the range [0, 100]
             */
            std::vector<int8_t> data = grid->data;
            std::vector<uint8_t> unsigned_data;
            unsigned_data.resize(data.size()); // allocate and fill vector to match num elements of data

            for (std::vector<int8_t>::size_type i = 0; i < data.size(); i++) {
                int8_t old_int = data[i];
                // assume unknown values (signed -1) are just 0
                // assumption is that unseen parts of the map have no obstacles until proven otherwise
                uint8_t new_int = (uint8_t)((old_int == -1) ? 0 : old_int);
                unsigned_data[(std::vector<uint8_t>::size_type) i] = new_int;
            }

            // Create the ROS Image Message

            sensor_msgs::Image converted_image;
            converted_image.width = cell_width;
            converted_image.height = cell_height;
            converted_image.encoding = sensor_msgs::image_encodings::MONO8;

            converted_image.step = cell_width;
            converted_image.data = unsigned_data;

            // Use CV Bridge to get the CvImagePtr

            cv_bridge::CvImagePtr cv_ptr;

            try {
                cv_ptr = cv_bridge::toCvCopy(converted_image, sensor_msgs::image_encodings::MONO8);
            } catch (cv_bridge::Exception& e) {
                ROS_ERROR("cv_bridge exception: %s", e.what());
            }

            // TODO cut down, shift images to align with perimeter from alert
            /*
             * Use the OpenCV cv::Rect intersection operator to find the
             *  relevant intersection of the full image with the base image
             *  Cut this out as a region of interest and paste it over the
             *  black base image
             * Return this image
             */
            int map_cell_origin_x = (int) (origin_x / resolution); // pixels
            int map_cell_origin_y = (int) (origin_y / resolution); // pixels

            cv::Rect map_region(map_cell_origin_x, map_cell_origin_y, cell_width, cell_height);
            ROS_DEBUG("map x %d y %d w %d h %d", map_region.x, map_region.y, map_region.width, map_region.height);

            int p_cell_origin_x = (int) (alert->min_x / resolution); // pixels
            int p_cell_origin_y = (int) (alert->min_y / resolution); // pixels
            int perim_width = (int) ((alert->max_x - alert->min_x) / resolution); // pixels
            int perim_height = (int) ((alert->max_y - alert->min_y) / resolution); // pixels
            ROS_DEBUG("perim w: %d h: %d", perim_width, perim_height);

            cv::Rect perimeter(p_cell_origin_x, p_cell_origin_y, perim_width, perim_height);
            ROS_DEBUG("perimeter x %d y %d w %d h %d", perimeter.x, perimeter.y, perimeter.width, perimeter.height);

            cv::Rect intersect = map_region & perimeter;

            ROS_DEBUG("I made my map rectangle");

            // shift intersection to the coordinates of the cropped image
            intersect.x -= map_cell_origin_x;
            intersect.y -= map_cell_origin_y;

            ROS_DEBUG("shift intersect 1 x %d y %d w %d h %d", intersect.x, intersect.y, intersect.width, intersect.height);

            // Assertion: 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows
            ROS_DEBUG("conditions 1: %d %d %d %d %d %d",
                0 <= intersect.x,
                0 <= intersect.width,
                intersect.x + intersect.width <= cv_ptr->image.cols,
                0 <= intersect.y,
                0 <= intersect.height,
                intersect.y + intersect.height <= cv_ptr->image.rows);
            // this is the subset of the two images from the map that should be copied into the base
            cv::Mat cropped_map = cv_ptr->image(intersect);

            ROS_DEBUG("cropped map");

            cv::Mat base(perim_height, perim_width, CV_8UC1, cv::Scalar(0));
            // shift intersection to the coordinates of the base image
            intersect.x = intersect.x + map_cell_origin_x - p_cell_origin_x;
            intersect.y = intersect.y + map_cell_origin_y - p_cell_origin_y;

            ROS_DEBUG("shift intersect 2 x %d y %d w %d h %d", intersect.x, intersect.y, intersect.width, intersect.height);
            ROS_DEBUG("base_info x 0 y 0 w %d h %d", base.cols, base.rows);

            // Assertion: 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows
            ROS_DEBUG("conditions 2: %d %d %d %d %d %d",
                0 <= intersect.x,
                0 <= intersect.width,
                intersect.x + intersect.width <= base.cols,
                0 <= intersect.y,
                0 <= intersect.height,
                intersect.y + intersect.height <= base.rows);
            ROS_DEBUG("conditions 2.1:\n0 <= %d\n0 <= %d\n%d <= %d\n0 <= %d\n0 <= %d\n%d <= %d",
                intersect.x,
                intersect.width,
                intersect.x + intersect.width, base.cols,
                0 <= intersect.y,
                0 <= intersect.height,
                intersect.y + intersect.height, base.rows);

            cv::Mat base_roi(base, intersect);

            ROS_DEBUG("shift intersect 2");

            // copy the map overlay to the base image
            if (intersect.area() > 0) {
                cropped_map.copyTo(base_roi);
                ROS_DEBUG("Overlay done");
            } else {
                ROS_DEBUG("No overlay (no area)");
            }

            cv_ptr->image = base;
            return cv_ptr;
        }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "DataController");
    DataController dp;
    dp.updated = false;
    ros::Rate r(30);

    dp.update();
    while(ros::ok()) {
        ros::spinOnce();
        if (dp.updated || true) {
            dp.update();
            ROS_INFO("Processed message data in loop");
        }
        // dp.pub_alert_status();
        r.sleep();
    }
    ROS_INFO("Data Processor Exited.");
    return 0;
}
<commit_msg>Fix runtime error of DataController<commit_after>#include <ros/ros.h>

#include <math.h>
#include <stdint.h>
#include <stdbool.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <opencv2/highgui/highgui.hpp>
#include <quirkd/Alert.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

class DataController {
    public:
        DataController()
            : it_(n_)
        {
            alert_pub_ = n_.advertise<quirkd::Alert>("/quirkd/alert/notification", 1);
            laser_sub_ = n_.subscribe("/base_scan", 1, &DataController::laserScanCB, this);
            ROS_INFO("WaitForService(\"static_map\");");
            ros::service::waitForService("static_map");
            static_map_client_ = n_.serviceClient<nav_msgs::GetMap>("static_map");
            ROS_INFO("WaitForService(\"dynamic_map\");");
            ros::service::waitForService("dynamic_map");
            dynamic_map_client_ = n_.serviceClient<nav_msgs::GetMap>("dynamic_map");
            static_image_pub_ = it_.advertise("/quirkd/test/static_image", 1);
            dynamic_image_pub_ = it_.advertise("/quirkd/test/dynamic_image", 1);
            heatmap_pub_ = it_.advertise("/quirkd/test/heatmap", 1);
        }
        void laserScanCB(const sensor_msgs::LaserScan msg) {
            ROS_INFO("Laser Scan Callback");
            updated = true;
            last_data = msg;
            try {
                tf_.lookupTransform("/map", "/base_laser_link", ros::Time(0), last_tf);
                ROS_INFO("tf success for /map to /base_laser_link");
            } catch (tf::TransformException &ex) {
                ROS_WARN("tf fetch failed. %s", ex.what());
            }
        }
        void update() {
            quirkd::Alert alert;
            alert.level = 0;
            alert.min_x = 0;
            alert.max_x = 1;
            alert.min_y = 0;
            alert.max_y = 1;
            updateAlertPerimeter(&alert, last_data, last_tf);

            ROS_INFO("Update Data Processor");
            nav_msgs::GetMap srv;
            cv_bridge::CvImagePtr static_image;
            cv_bridge::CvImagePtr dynamic_image;
            if (static_map_client_.call(srv)) {
                ROS_INFO("Successfull call static map");
                nav_msgs::OccupancyGrid og = srv.response.map;
                static_image = this->gridToCroppedCvImage(&og, &alert);

                static_image_pub_.publish(static_image->toImageMsg());

            } else {
                ROS_WARN("Failed to get static map");
            }
            if (dynamic_map_client_.call(srv)) {
                ROS_INFO("Successfull call dynamic map");
                nav_msgs::OccupancyGrid og = srv.response.map;
                dynamic_image = this->gridToCroppedCvImage(&og, &alert);

                dynamic_image_pub_.publish(dynamic_image->toImageMsg());
            } else {
                ROS_WARN("Failed to get dynamic map");
                dynamic_image = static_image;
            }

            // two images (static (base) image and dynamic image)
            // perimeter encoded as part of the alert and images aligned to alert perimeter

            /* Compute difference
             * Steps:
             * - manhattan distance
             */

            // Manhattan Distance
            cv::Mat absdiff;
            cv::absdiff(static_image->image, dynamic_image->image, absdiff);

            // use absdiff as a mask to visualize difference with "heatmap" (bgr)
            cv::Mat redBase(static_image->image.rows, static_image->image.cols, CV_8UC3, cv::Scalar(0, 0, 255));
            cv::Mat byChannel[3];
            cv::split(redBase, byChannel);
            // byChannel[2] = absdiff;
            cv::merge(byChannel, 3, redBase);

            ROS_INFO("Split and merge done");
            // cv_bridge::CvImage heatmap(header, sensor_msgs::image_encodings::BGR8, byChannel);
            cv_bridge::CvImage heatmap;
            ROS_INFO("Create heatmap");
            heatmap.header = static_image->header;
            ROS_INFO("Add header");
            heatmap.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
            // problem in this step
            ROS_INFO("set encoding");
            heatmap.image = redBase;
            ROS_INFO("set image done redBase");
            heatmap_pub_.publish(heatmap.toImageMsg());
            ROS_INFO("publish heatmap");

            alert.level = cv::sum(absdiff)[0];

            // TODO publish difference
            ROS_INFO("The two images have a difference of %d", alert.level);
            alert_pub_.publish(alert);

            updated = false;
        }
        bool updated;
        sensor_msgs::LaserScan last_data;
        tf::StampedTransform last_tf;
    private:
        ros::NodeHandle n_;
        image_transport::ImageTransport it_;
        image_transport::Publisher static_image_pub_;
        image_transport::Publisher dynamic_image_pub_;
        image_transport::Publisher heatmap_pub_;
        ros::Publisher alert_pub_;
        ros::Subscriber laser_sub_;
        ros::ServiceClient dynamic_map_client_;
        ros::ServiceClient static_map_client_;
        tf::TransformListener tf_;
        void updateAlertPerimeter(quirkd::Alert* alert, const sensor_msgs::LaserScan scan, const tf::StampedTransform tf) {
            double base_x = tf.getOrigin().x();
            double base_y = tf.getOrigin().y();
            double r, p, base_heading;
            tf::Matrix3x3 m(tf.getRotation());
            m.getRPY(r, p, base_heading);

            double scan_min = base_heading + scan.angle_min;
            double scan_inc = scan.angle_increment;
            double heading = scan_min;

            alert->min_x = base_x;
            alert->max_x = base_x;
            alert->min_y = base_y;
            alert->max_y = base_y;

            double live_x, live_y;

            for (int i = 0; i < scan.ranges.size(); i++) {
                double dist = scan.ranges[i];

                live_x = base_x + dist * cos(heading);
                live_y = base_y + dist * sin(heading);
                alert->min_x = std::min(live_x, double(alert->min_x));
                alert->max_x = std::max(live_x, double(alert->max_x));
                alert->min_y = std::min(live_y, double(alert->min_y));
                alert->max_y = std::max(live_y, double(alert->max_y));

                heading += scan_inc;
            }
            double padding = 0.5;
            alert->min_x += -padding;
            alert->min_y += -padding;
            alert->max_x += padding;
            alert->max_y += padding;
        }
        cv_bridge::CvImagePtr gridToCroppedCvImage(nav_msgs::OccupancyGrid* grid, quirkd::Alert* alert) {
            // Unpack the Occupancy Grid

            nav_msgs::MapMetaData info = grid->info;
            float resolution = info.resolution; // meters per pixel
            int cell_width = info.width;
            float map_width = cell_width * resolution;
            int cell_height = info.height;
            float map_height = cell_height * resolution;
            // I'm assuming the map isn't rotated right now because that could be really complex
            float origin_x = info.origin.position.x;
            float origin_y = info.origin.position.x;
            /*
             * From documentation:
             * The map data in row major order, starting at 0,0.
             * Valid values are in the range [0, 100]
             */
            std::vector<int8_t> data = grid->data;
            std::vector<uint8_t> unsigned_data;
            unsigned_data.resize(data.size()); // allocate and fill vector to match num elements of data

            for (std::vector<int8_t>::size_type i = 0; i < data.size(); i++) {
                int8_t old_int = data[i];
                // assume unknown values (signed -1) are just 0
                // assumption is that unseen parts of the map have no obstacles until proven otherwise
                uint8_t new_int = (uint8_t)((old_int == -1) ? 0 : old_int);
                unsigned_data[(std::vector<uint8_t>::size_type) i] = new_int;
            }

            // Create the ROS Image Message

            sensor_msgs::Image converted_image;
            converted_image.width = cell_width;
            converted_image.height = cell_height;
            converted_image.encoding = sensor_msgs::image_encodings::MONO8;

            converted_image.step = cell_width;
            converted_image.data = unsigned_data;

            // Use CV Bridge to get the CvImagePtr

            cv_bridge::CvImagePtr cv_ptr;

            try {
                cv_ptr = cv_bridge::toCvCopy(converted_image, sensor_msgs::image_encodings::MONO8);
            } catch (cv_bridge::Exception& e) {
                ROS_ERROR("cv_bridge exception: %s", e.what());
            }

            // TODO cut down, shift images to align with perimeter from alert
            /*
             * Use the OpenCV cv::Rect intersection operator to find the
             *  relevant intersection of the full image with the base image
             *  Cut this out as a region of interest and paste it over the
             *  black base image
             * Return this image
             */
            int map_cell_origin_x = (int) (origin_x / resolution); // pixels
            int map_cell_origin_y = (int) (origin_y / resolution); // pixels

            cv::Rect map_region(map_cell_origin_x, map_cell_origin_y, cell_width, cell_height);
            ROS_DEBUG("map x %d y %d w %d h %d", map_region.x, map_region.y, map_region.width, map_region.height);

            int p_cell_origin_x = (int) (alert->min_x / resolution); // pixels
            int p_cell_origin_y = (int) (alert->min_y / resolution); // pixels
            int perim_width = (int) ((alert->max_x - alert->min_x) / resolution); // pixels
            int perim_height = (int) ((alert->max_y - alert->min_y) / resolution); // pixels
            ROS_DEBUG("perim w: %d h: %d", perim_width, perim_height);

            cv::Rect perimeter(p_cell_origin_x, p_cell_origin_y, perim_width, perim_height);
            ROS_DEBUG("perimeter x %d y %d w %d h %d", perimeter.x, perimeter.y, perimeter.width, perimeter.height);

            cv::Rect intersect = map_region & perimeter;

            ROS_DEBUG("I made my map rectangle");

            // shift intersection to the coordinates of the cropped image
            intersect.x -= map_cell_origin_x;
            intersect.y -= map_cell_origin_y;

            ROS_DEBUG("shift intersect 1 x %d y %d w %d h %d", intersect.x, intersect.y, intersect.width, intersect.height);

            // Assertion: 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows
            ROS_DEBUG("conditions 1: %d %d %d %d %d %d",
                0 <= intersect.x,
                0 <= intersect.width,
                intersect.x + intersect.width <= cv_ptr->image.cols,
                0 <= intersect.y,
                0 <= intersect.height,
                intersect.y + intersect.height <= cv_ptr->image.rows);
            // this is the subset of the two images from the map that should be copied into the base
            cv::Mat cropped_map = cv_ptr->image(intersect);

            ROS_DEBUG("cropped map");

            cv::Mat base(perim_height, perim_width, CV_8UC1, cv::Scalar(0));
            // shift intersection to the coordinates of the base image
            intersect.x = intersect.x + map_cell_origin_x - p_cell_origin_x;
            intersect.y = intersect.y + map_cell_origin_y - p_cell_origin_y;

            ROS_DEBUG("shift intersect 2 x %d y %d w %d h %d", intersect.x, intersect.y, intersect.width, intersect.height);
            ROS_DEBUG("base_info x 0 y 0 w %d h %d", base.cols, base.rows);

            // Assertion: 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows
            ROS_DEBUG("conditions 2: %d %d %d %d %d %d",
                0 <= intersect.x,
                0 <= intersect.width,
                intersect.x + intersect.width <= base.cols,
                0 <= intersect.y,
                0 <= intersect.height,
                intersect.y + intersect.height <= base.rows);
            ROS_DEBUG("conditions 2.1:\n0 <= %d\n0 <= %d\n%d <= %d\n0 <= %d\n0 <= %d\n%d <= %d",
                intersect.x,
                intersect.width,
                intersect.x + intersect.width, base.cols,
                0 <= intersect.y,
                0 <= intersect.height,
                intersect.y + intersect.height, base.rows);

            cv::Mat base_roi(base, intersect);

            ROS_DEBUG("shift intersect 2");

            // copy the map overlay to the base image
            if (intersect.area() > 0) {
                cropped_map.copyTo(base_roi);
                ROS_DEBUG("Overlay done");
            } else {
                ROS_DEBUG("No overlay (no area)");
            }

            cv_ptr->image = base;
            return cv_ptr;
        }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "DataController");
    DataController dp;
    dp.updated = false;
    ros::Rate r(30);

    dp.update();
    while(ros::ok()) {
        ros::spinOnce();
        if (dp.updated || true) {
            dp.update();
            ROS_INFO("Processed message data in loop");
        }
        // dp.pub_alert_status();
        r.sleep();
    }
    ROS_INFO("Data Processor Exited.");
    return 0;
}
<|endoftext|>


<commit_before>// -*- Mode:C++ -*-

/**************************************************************************************************/
/*                                                                                */
/* Copyright (C) 2015 University of Hull                                                          */
/*                                                                                */
/**************************************************************************************************/
/*                                                                                */
/*  module     :  app/oglplus/viewer/ssbo.cpp                                                     */
/*  project    :                                                                                */
/*  description:                                                                                */
/*                                                                                */
/**************************************************************************************************/

// include i/f header

#include "ssbo.hpp"

// includes, system

//#include <>

// includes, project

//#include <>

#define UKACHULLDCS_USE_TRACE
#undef UKACHULLDCS_USE_TRACE
#include <support/trace.hpp>

// internal unnamed namespace

namespace {

  // types, internal (class, enum, struct, union, typedef)

  // variables, internal

  // functions, internal

} // namespace {

namespace buffer {

  // variables, exported

  /* static */ slot_manager base::slot_mgr(96);

  // functions, exported

  /* explicit */
  slot_manager::slot_manager(unsigned a)
    : slots_(a, false)
  {
    TRACE("buffer::slot_manager::slot_manager");
  }

  unsigned
  slot_manager::alloc()
  {
    TRACE("buffer::slot_manager::alloc");

    unsigned result(0);

    for (unsigned i(0); i < slots_.size(); ++i) {
      if (false == slots_[i]) {
        slots_[i] = true;
        result    = i;

        break;
      }
    }

#if defined(UKACHULLDCS_USE_TRACE)
    std::cout << support::trace::prefix() << "buffer::slot_manager::alloc: "
              << "slot:" << result
              << '\n';
#endif

    return result;
  }

  void
  slot_manager::release(unsigned a)
  {
    TRACE("buffer::slot_manager::release");

#if defined(UKACHULLDCS_USE_TRACE)
    std::cout << support::trace::prefix() << "buffer::slot_manager::release: "
              << "slot:" << a
              << '\n';
#endif

    slots_[a] = false;
  }

  /* explicit */
  base::base(std::string const& a, oglplus::Program& b)
    : name_(a),
      prg_ (b),
      buf_ ()
  {
    TRACE("buffer::base::base");
  }

} // namespace buffer {
<commit_msg>update<commit_after>// -*- Mode:C++ -*-

/**************************************************************************************************/
/*                                                                                */
/* Copyright (C) 2015 University of Hull                                                          */
/*                                                                                */
/**************************************************************************************************/
/*                                                                                */
/*  module     :  app/oglplus/viewer/ssbo.cpp                                                     */
/*  project    :                                                                                */
/*  description:                                                                                */
/*                                                                                */
/**************************************************************************************************/

// include i/f header

#include "ssbo.hpp"

// includes, system

//#include <>

// includes, project

//#include <>

#define UKACHULLDCS_USE_TRACE
#undef UKACHULLDCS_USE_TRACE
#include <support/trace.hpp>

// internal unnamed namespace

namespace {

  // types, internal (class, enum, struct, union, typedef)

  // variables, internal

  // functions, internal

} // namespace {

namespace buffer {

  // variables, exported

  /* static */ slot_manager base::slot_mgr(96);

  // functions, exported

  /* explicit */
  slot_manager::slot_manager(unsigned a)
    : slots_(a, false)
  {
    TRACE("buffer::slot_manager::slot_manager");
  }

  unsigned
  slot_manager::alloc()
  {
    TRACE("buffer::slot_manager::alloc");

    unsigned result(0);

    for (unsigned i(0); i < slots_.size(); ++i) {
      if (false == slots_[i]) {
        slots_[i] = true;
        result    = i;

        break;
      }
    }

#if defined(UKACHULLDCS_USE_TRACE)
    std::cout << support::trace::prefix() << "buffer::slot_manager::alloc: "
              << "slot:" << result
              << '\n';
#endif

#if 1
    {
#define MKT(a) std::make_tuple(#a, a, -1)
      std::array<std::tuple<char const*, signed const, signed>, 219> limits = {
        {
          MKT(GL_ACTIVE_TEXTURE),
          MKT(GL_ALIASED_LINE_WIDTH_RANGE),
          MKT(GL_ARRAY_BUFFER_BINDING),
          MKT(GL_BLEND),
          //MKT(GL_BLEND_COLOR),
          MKT(GL_BLEND_DST_ALPHA),
          MKT(GL_BLEND_DST_RGB),
          MKT(GL_BLEND_EQUATION_ALPHA),
          MKT(GL_BLEND_EQUATION_RGB),
          MKT(GL_BLEND_SRC_ALPHA),
          MKT(GL_BLEND_SRC_RGB),
          //MKT(GL_COLOR_CLEAR_VALUE),
          MKT(GL_COLOR_LOGIC_OP),
          //MKT(GL_COLOR_WRITEMASK),
          //MKT(GL_COMPRESSED_TEXTURE_FORMATS),
          MKT(GL_CONTEXT_FLAGS),
          MKT(GL_CULL_FACE),
          MKT(GL_CURRENT_PROGRAM),
          MKT(GL_DEBUG_GROUP_STACK_DEPTH),
          MKT(GL_DEPTH_CLEAR_VALUE),
          MKT(GL_DEPTH_FUNC),
          MKT(GL_DEPTH_RANGE),
          MKT(GL_DEPTH_TEST),
          MKT(GL_DEPTH_WRITEMASK),
          MKT(GL_DISPATCH_INDIRECT_BUFFER_BINDING),
          MKT(GL_DITHER),
          MKT(GL_DOUBLEBUFFER),
          MKT(GL_DRAW_BUFFER),
          MKT(GL_DRAW_FRAMEBUFFER_BINDING),
          MKT(GL_ELEMENT_ARRAY_BUFFER_BINDING),
          MKT(GL_FRAGMENT_SHADER_DERIVATIVE_HINT),
          MKT(GL_IMPLEMENTATION_COLOR_READ_FORMAT),
          MKT(GL_IMPLEMENTATION_COLOR_READ_TYPE),
          MKT(GL_LAYER_PROVOKING_VERTEX),
          MKT(GL_LINE_SMOOTH),
          MKT(GL_LINE_SMOOTH_HINT),
          MKT(GL_LINE_WIDTH),
          MKT(GL_LOGIC_OP_MODE),
          MKT(GL_MAJOR_VERSION),
          MKT(GL_MAX_3D_TEXTURE_SIZE),
          MKT(GL_MAX_ARRAY_TEXTURE_LAYERS),
          MKT(GL_MAX_CLIP_DISTANCES),
          MKT(GL_MAX_COLOR_TEXTURE_SAMPLES),
          MKT(GL_MAX_COMBINED_ATOMIC_COUNTERS),
          MKT(GL_MAX_COMBINED_COMPUTE_UNIFORM_COMPONENTS),
          MKT(GL_MAX_COMBINED_FRAGMENT_UNIFORM_COMPONENTS),
          MKT(GL_MAX_COMBINED_GEOMETRY_UNIFORM_COMPONENTS),
          MKT(GL_MAX_COMBINED_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_COMBINED_TEXTURE_IMAGE_UNITS),
          MKT(GL_MAX_COMBINED_UNIFORM_BLOCKS),
          MKT(GL_MAX_COMBINED_VERTEX_UNIFORM_COMPONENTS),
          MKT(GL_MAX_COMPUTE_ATOMIC_COUNTERS),
          MKT(GL_MAX_COMPUTE_ATOMIC_COUNTER_BUFFERS),
          MKT(GL_MAX_COMPUTE_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_COMPUTE_TEXTURE_IMAGE_UNITS),
          MKT(GL_MAX_COMPUTE_UNIFORM_BLOCKS),
          MKT(GL_MAX_COMPUTE_UNIFORM_COMPONENTS),
          MKT(GL_MAX_COMPUTE_WORK_GROUP_COUNT),
          MKT(GL_MAX_COMPUTE_WORK_GROUP_INVOCATIONS),
          MKT(GL_MAX_COMPUTE_WORK_GROUP_SIZE),
          MKT(GL_MAX_CUBE_MAP_TEXTURE_SIZE),
          MKT(GL_MAX_DEBUG_GROUP_STACK_DEPTH),
          MKT(GL_MAX_DEPTH_TEXTURE_SAMPLES),
          MKT(GL_MAX_DRAW_BUFFERS),
          MKT(GL_MAX_DUAL_SOURCE_DRAW_BUFFERS),
          MKT(GL_MAX_ELEMENTS_INDICES),
          MKT(GL_MAX_ELEMENTS_VERTICES),
          MKT(GL_MAX_ELEMENT_INDEX),
          MKT(GL_MAX_FRAGMENT_ATOMIC_COUNTERS),
          MKT(GL_MAX_FRAGMENT_INPUT_COMPONENTS),
          MKT(GL_MAX_FRAGMENT_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_FRAGMENT_UNIFORM_BLOCKS),
          MKT(GL_MAX_FRAGMENT_UNIFORM_COMPONENTS),
          MKT(GL_MAX_FRAGMENT_UNIFORM_VECTORS),
          MKT(GL_MAX_FRAMEBUFFER_HEIGHT),
          MKT(GL_MAX_FRAMEBUFFER_LAYERS),
          MKT(GL_MAX_FRAMEBUFFER_SAMPLES),
          MKT(GL_MAX_FRAMEBUFFER_WIDTH),
          MKT(GL_MAX_GEOMETRY_ATOMIC_COUNTERS),
          MKT(GL_MAX_GEOMETRY_INPUT_COMPONENTS),
          MKT(GL_MAX_GEOMETRY_OUTPUT_COMPONENTS),
          MKT(GL_MAX_GEOMETRY_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_GEOMETRY_TEXTURE_IMAGE_UNITS),
          MKT(GL_MAX_GEOMETRY_UNIFORM_BLOCKS),
          MKT(GL_MAX_GEOMETRY_UNIFORM_COMPONENTS),
          MKT(GL_MAX_INTEGER_SAMPLES),
          MKT(GL_MAX_LABEL_LENGTH),
          MKT(GL_MAX_PROGRAM_TEXEL_OFFSET),
          MKT(GL_MAX_RECTANGLE_TEXTURE_SIZE),
          MKT(GL_MAX_RENDERBUFFER_SIZE),
          MKT(GL_MAX_SAMPLE_MASK_WORDS),
          MKT(GL_MAX_SERVER_WAIT_TIMEOUT),
          MKT(GL_MAX_SHADER_STORAGE_BUFFER_BINDINGS),
          MKT(GL_MAX_TESS_CONTROL_ATOMIC_COUNTERS),
          MKT(GL_MAX_TESS_CONTROL_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_TESS_EVALUATION_ATOMIC_COUNTERS),
          MKT(GL_MAX_TESS_EVALUATION_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_TEXTURE_BUFFER_SIZE),
          MKT(GL_MAX_TEXTURE_IMAGE_UNITS),
          MKT(GL_MAX_TEXTURE_LOD_BIAS),
          MKT(GL_MAX_TEXTURE_SIZE),
          MKT(GL_MAX_UNIFORM_BLOCK_SIZE),
          MKT(GL_MAX_UNIFORM_BUFFER_BINDINGS),
          MKT(GL_MAX_UNIFORM_LOCATIONS),
          MKT(GL_MAX_VARYING_COMPONENTS),
          MKT(GL_MAX_VARYING_FLOATS),
          MKT(GL_MAX_VARYING_VECTORS),
          MKT(GL_MAX_VERTEX_ATOMIC_COUNTERS),
          MKT(GL_MAX_VERTEX_ATTRIBS),
          MKT(GL_MAX_VERTEX_ATTRIB_BINDINGS),
          MKT(GL_MAX_VERTEX_ATTRIB_RELATIVE_OFFSET),
          MKT(GL_MAX_VERTEX_OUTPUT_COMPONENTS),
          MKT(GL_MAX_VERTEX_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_VERTEX_SHADER_STORAGE_BLOCKS),
          MKT(GL_MAX_VERTEX_TEXTURE_IMAGE_UNITS),
          MKT(GL_MAX_VERTEX_UNIFORM_BLOCKS),
          MKT(GL_MAX_VERTEX_UNIFORM_COMPONENTS),
          MKT(GL_MAX_VERTEX_UNIFORM_VECTORS),
          MKT(GL_MAX_VIEWPORTS),
          MKT(GL_MAX_VIEWPORT_DIMS),
          MKT(GL_MINOR_VERSION),
          MKT(GL_MIN_MAP_BUFFER_ALIGNMENT),
          MKT(GL_MIN_PROGRAM_TEXEL_OFFSET),
          MKT(GL_NUM_COMPRESSED_TEXTURE_FORMATS),
          MKT(GL_NUM_EXTENSIONS),
          MKT(GL_NUM_PROGRAM_BINARY_FORMATS),
          MKT(GL_NUM_SHADER_BINARY_FORMATS),
          MKT(GL_PACK_ALIGNMENT),
          MKT(GL_PACK_IMAGE_HEIGHT),
          MKT(GL_PACK_LSB_FIRST),
          MKT(GL_PACK_ROW_LENGTH),
          MKT(GL_PACK_SKIP_IMAGES),
          MKT(GL_PACK_SKIP_PIXELS),
          MKT(GL_PACK_SKIP_ROWS),
          MKT(GL_PACK_SWAP_BYTES),
          MKT(GL_PIXEL_PACK_BUFFER_BINDING),
          MKT(GL_PIXEL_UNPACK_BUFFER_BINDING),
          MKT(GL_POINT_FADE_THRESHOLD_SIZE),
          MKT(GL_POINT_SIZE),
          MKT(GL_POINT_SIZE_GRANULARITY),
          MKT(GL_POINT_SIZE_RANGE),
          MKT(GL_POLYGON_OFFSET_FACTOR),
          MKT(GL_POLYGON_OFFSET_FILL),
          MKT(GL_POLYGON_OFFSET_LINE),
          MKT(GL_POLYGON_OFFSET_POINT),
          MKT(GL_POLYGON_OFFSET_UNITS),
          MKT(GL_POLYGON_SMOOTH),
          MKT(GL_POLYGON_SMOOTH_HINT),
          MKT(GL_PRIMITIVE_RESTART_INDEX),
          MKT(GL_PROGRAM_BINARY_FORMATS),
          MKT(GL_PROGRAM_PIPELINE_BINDING),
          MKT(GL_PROGRAM_POINT_SIZE),
          MKT(GL_PROVOKING_VERTEX),
          MKT(GL_READ_BUFFER),
          MKT(GL_READ_FRAMEBUFFER_BINDING),
          MKT(GL_RENDERBUFFER_BINDING),
          MKT(GL_SAMPLER_BINDING),
          MKT(GL_SAMPLES),
          MKT(GL_SAMPLE_BUFFERS),
          MKT(GL_SAMPLE_COVERAGE_INVERT),
          MKT(GL_SAMPLE_COVERAGE_VALUE),
          //MKT(GL_SCISSOR_BOX),
          MKT(GL_SCISSOR_TEST),
          MKT(GL_SHADER_COMPILER),
          MKT(GL_SHADER_STORAGE_BUFFER_BINDING),
          MKT(GL_SHADER_STORAGE_BUFFER_OFFSET_ALIGNMENT),
          MKT(GL_SHADER_STORAGE_BUFFER_SIZE),
          MKT(GL_SHADER_STORAGE_BUFFER_START),
          MKT(GL_SMOOTH_LINE_WIDTH_GRANULARITY),
          MKT(GL_SMOOTH_LINE_WIDTH_RANGE),
          MKT(GL_STENCIL_BACK_FAIL),
          MKT(GL_STENCIL_BACK_FUNC),
          MKT(GL_STENCIL_BACK_PASS_DEPTH_FAIL),
          MKT(GL_STENCIL_BACK_PASS_DEPTH_PASS),
          MKT(GL_STENCIL_BACK_REF),
          MKT(GL_STENCIL_BACK_VALUE_MASK),
          MKT(GL_STENCIL_BACK_WRITEMASK),
          MKT(GL_STENCIL_CLEAR_VALUE),
          MKT(GL_STENCIL_FAIL),
          MKT(GL_STENCIL_FUNC),
          MKT(GL_STENCIL_PASS_DEPTH_FAIL),
          MKT(GL_STENCIL_PASS_DEPTH_PASS),
          MKT(GL_STENCIL_REF),
          MKT(GL_STENCIL_TEST),
          MKT(GL_STENCIL_VALUE_MASK),
          MKT(GL_STENCIL_WRITEMASK),
          MKT(GL_STEREO),
          MKT(GL_SUBPIXEL_BITS),
          MKT(GL_TEXTURE_2D),
          MKT(GL_TEXTURE_BINDING_1D),
          MKT(GL_TEXTURE_BINDING_1D_ARRAY),
          MKT(GL_TEXTURE_BINDING_2D),
          MKT(GL_TEXTURE_BINDING_2D_ARRAY),
          MKT(GL_TEXTURE_BINDING_2D_MULTISAMPLE),
          MKT(GL_TEXTURE_BINDING_2D_MULTISAMPLE_ARRAY),
          MKT(GL_TEXTURE_BINDING_3D),
          MKT(GL_TEXTURE_BINDING_BUFFER),
          MKT(GL_TEXTURE_BINDING_CUBE_MAP),
          MKT(GL_TEXTURE_BINDING_RECTANGLE),
          MKT(GL_TEXTURE_BUFFER_OFFSET_ALIGNMENT),
          MKT(GL_TEXTURE_COMPRESSION_HINT),
          MKT(GL_TIMESTAMP),
          MKT(GL_TRANSFORM_FEEDBACK_BUFFER_BINDING),
          MKT(GL_TRANSFORM_FEEDBACK_BUFFER_SIZE),
          MKT(GL_TRANSFORM_FEEDBACK_BUFFER_START),
          MKT(GL_UNIFORM_BUFFER_BINDING),
          MKT(GL_UNIFORM_BUFFER_OFFSET_ALIGNMENT),
          MKT(GL_UNIFORM_BUFFER_SIZE),
          MKT(GL_UNIFORM_BUFFER_START),
          MKT(GL_UNPACK_ALIGNMENT),
          MKT(GL_UNPACK_IMAGE_HEIGHT),
          MKT(GL_UNPACK_LSB_FIRST),
          MKT(GL_UNPACK_ROW_LENGTH),
          MKT(GL_UNPACK_SKIP_IMAGES),
          MKT(GL_UNPACK_SKIP_PIXELS),
          MKT(GL_UNPACK_SKIP_ROWS),
          MKT(GL_UNPACK_SWAP_BYTES),
          MKT(GL_VERTEX_ARRAY_BINDING),
          MKT(GL_VERTEX_BINDING_DIVISOR),
          MKT(GL_VERTEX_BINDING_OFFSET),
          MKT(GL_VERTEX_BINDING_STRIDE),
          //MKT(GL_VIEWPORT),
          MKT(GL_VIEWPORT_BOUNDS_RANGE),
          MKT(GL_VIEWPORT_INDEX_PROVOKING_VERTEX),
          MKT(GL_VIEWPORT_SUBPIXEL_BITS),
        }
      };
#undef MKT

      for (auto& l : limits) {
        ::glGetIntegerv(std::get<1>(l), &std::get<2>(l));
      }

      std::cout << support::trace::prefix() << "buffer::slot_manager::alloc: ";

      std::string const indent(support::trace::prefix().length(), ' ');

      for (auto const& l : limits) {
        std::cout << '\n' << indent
                  << std::get<0>(l) << ':' << std::get<2>(l);
      }

      std::cout << '\n';
    }
#endif

    return result;
  }

  void
  slot_manager::release(unsigned a)
  {
    TRACE("buffer::slot_manager::release");

#if defined(UKACHULLDCS_USE_TRACE)
    std::cout << support::trace::prefix() << "buffer::slot_manager::release: "
              << "slot:" << a
              << '\n';
#endif

    slots_[a] = false;
  }

  /* explicit */
  base::base(std::string const& a, oglplus::Program& b)
    : name_(a),
      prg_ (b),
      buf_ ()
  {
    TRACE("buffer::base::base");
  }

} // namespace buffer {
<|endoftext|>


<commit_before>// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/steering-method/spline.hh>

#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/pinocchio/configuration.hh>

#include <hpp/constraints/svd.hh>

#include <hpp/core/problem.hh>
#include <hpp/core/path/spline.hh>

namespace hpp {
  namespace core {
    namespace steeringMethod {
      template <int _PB, int _SO>
      PathPtr_t Spline<_PB, _SO>::impl_compute (ConfigurationIn_t q1,
          ConfigurationIn_t q2) const
      {
        enum {
          NDerivativeConstraintPerSide = int( (SplineOrder + 1 - 2) / 2)
        };
        typedef Eigen::Matrix<value_type, Eigen::Dynamic, NDerivativeConstraintPerSide> DerMatrix_t;
        typedef typename DerMatrix_t::ConstantReturnType DefaultDerivatives_t;

        DefaultDerivatives_t defaultDer (DerMatrix_t::Zero(device_.lock()->numberDof(), NDerivativeConstraintPerSide));
        std::vector<int> orders (NDerivativeConstraintPerSide);
        for (std::size_t i = 0; i < NDerivativeConstraintPerSide; ++i) orders[i] = int(i + 1);
        return impl_compute (q1, orders, defaultDer, q2, orders, defaultDer);
      }

      template <int _PB, int _SO>
      PathPtr_t Spline<_PB, _SO>::steer (
          ConfigurationIn_t q1, std::vector<int> order1,  matrixIn_t derivatives1,
          ConfigurationIn_t q2, std::vector<int> order2,  matrixIn_t derivatives2) const
      {
        // TODO check the size of the derivatives.
        // If too small, they should be extended with columns of zeros.
        return impl_compute (q1, order1, derivatives1, q2, order2, derivatives2);
      }

      template <int _PB, int _SO>
      template <typename Derived>
      PathPtr_t Spline<_PB, _SO>::impl_compute (
              ConfigurationIn_t q1, std::vector<int> order1,  const Eigen::MatrixBase<Derived>& derivatives1,
              ConfigurationIn_t q2, std::vector<int> order2,  const Eigen::MatrixBase<Derived>& derivatives2) const
      {
        // Compute the decomposition
        // typedef Eigen::Matrix<value_type, SplineOrder+1, SplineOrder+1> ConstraintMatrix_t;
        typedef Eigen::Matrix<value_type, Eigen::Dynamic, SplineOrder+1, Eigen::RowMajor> ConstraintMatrix_t;
        typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RhsMatrix_t;

        SplinePathPtr_t p = SplinePath::create
          (device_.lock(), interval_t (0, 1), constraints());

        const size_type nbConstraints = 2 + derivatives1.cols() + derivatives2.cols();
        ConstraintMatrix_t coeffs (nbConstraints, SplineOrder+1);
        RhsMatrix_t rhs (nbConstraints, device_.lock()->numberDof());

        p->base(q1); // TODO use the center ?

        // Compute the matrices
        // TODO calls to basisFunctionDerivative could be cached as they do not
        // depend on the inputs.
        p->basisFunctionDerivative(0, 0, coeffs.row(0));
        pinocchio::difference<hpp::pinocchio::LieGroupTpl>(device_.lock(), q1, p->base(), rhs.row(0));
        for (std::size_t i = 0; i < order1.size(); ++i)
          p->basisFunctionDerivative(order1[i], 0, coeffs.row(i+1));
        rhs.middleRows(1, order1.size()).transpose() = derivatives1;

        size_type row = 1 + order1.size();
        p->basisFunctionDerivative(0, 1, coeffs.row(row));
        pinocchio::difference<hpp::pinocchio::LieGroupTpl>(device_.lock(), q2, p->base(), rhs.row(row));
        ++row;
        for (std::size_t i = 0; i < order2.size(); ++i)
          p->basisFunctionDerivative(order2[i], 1, coeffs.row(i+row));
        rhs.middleRows(row, order2.size()).transpose() = derivatives2;

        // Solve the problem
        // coeffs * P = rhs
        typedef Eigen::JacobiSVD < ConstraintMatrix_t > SVD_t;
        SVD_t svd (coeffs, Eigen::ComputeFullU | Eigen::ComputeFullV);
        p->parameters (svd.solve(rhs));

        return p;
      }

      template <int _PB, int _SO>
      Spline<_PB, _SO>::Spline (const Problem& problem) :
        SteeringMethod (problem), device_ (problem.robot ())
      {}

      /// Copy constructor
      template <int _PB, int _SO>
      Spline<_PB, _SO>::Spline (const Spline& other) :
        SteeringMethod (other), device_ (other.device_)
      {}

      template class Spline<path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
      template class Spline<path::CanonicalPolynomeBasis, 2>;
      template class Spline<path::CanonicalPolynomeBasis, 3>;
      template class Spline<path::BernsteinBasis, 1>; // equivalent to StraightPath
      template class Spline<path::BernsteinBasis, 2>;
      template class Spline<path::BernsteinBasis, 3>;
    } // namespace steeringMethod
  } // namespace core
} // namespace hpp
<commit_msg>Add some comments<commit_after>// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/steering-method/spline.hh>

#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/pinocchio/configuration.hh>

#include <hpp/constraints/svd.hh>

#include <hpp/core/problem.hh>
#include <hpp/core/path/spline.hh>

namespace hpp {
  namespace core {
    namespace steeringMethod {
      template <int _PB, int _SO>
      PathPtr_t Spline<_PB, _SO>::impl_compute (ConfigurationIn_t q1,
          ConfigurationIn_t q2) const
      {
        enum {
          NDerivativeConstraintPerSide = int( (SplineOrder + 1 - 2) / 2)
        };
        typedef Eigen::Matrix<value_type, Eigen::Dynamic, NDerivativeConstraintPerSide> DerMatrix_t;
        typedef typename DerMatrix_t::ConstantReturnType DefaultDerivatives_t;

        DefaultDerivatives_t defaultDer (DerMatrix_t::Zero(device_.lock()->numberDof(), NDerivativeConstraintPerSide));
        std::vector<int> orders (NDerivativeConstraintPerSide);
        for (std::size_t i = 0; i < NDerivativeConstraintPerSide; ++i) orders[i] = int(i + 1);
        return impl_compute (q1, orders, defaultDer, q2, orders, defaultDer);
      }

      template <int _PB, int _SO>
      PathPtr_t Spline<_PB, _SO>::steer (
          ConfigurationIn_t q1, std::vector<int> order1,  matrixIn_t derivatives1,
          ConfigurationIn_t q2, std::vector<int> order2,  matrixIn_t derivatives2) const
      {
        // TODO check the size of the derivatives.
        // If too small, they should be extended with columns of zeros.
        return impl_compute (q1, order1, derivatives1, q2, order2, derivatives2);
      }

      template <int _PB, int _SO>
      template <typename Derived>
      PathPtr_t Spline<_PB, _SO>::impl_compute (
              ConfigurationIn_t q1, std::vector<int> order1,  const Eigen::MatrixBase<Derived>& derivatives1,
              ConfigurationIn_t q2, std::vector<int> order2,  const Eigen::MatrixBase<Derived>& derivatives2) const
      {
        // Compute the decomposition
        // typedef Eigen::Matrix<value_type, SplineOrder+1, SplineOrder+1> ConstraintMatrix_t;
        typedef Eigen::Matrix<value_type, Eigen::Dynamic, SplineOrder+1, Eigen::RowMajor> ConstraintMatrix_t;
        typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RhsMatrix_t;

        SplinePathPtr_t p = SplinePath::create
          (device_.lock(), interval_t (0, 1), constraints());

        const size_type nbConstraints = 2 + derivatives1.cols() + derivatives2.cols();
        ConstraintMatrix_t coeffs (nbConstraints, SplineOrder+1);
        RhsMatrix_t rhs (nbConstraints, device_.lock()->numberDof());

        p->base(q1); // TODO use the center ?
        // p->base(device_.lock()->neutralConfiguration()); // TODO use the center ?
        // Configuration_t qmiddle (q1.size());
        // pinocchio::interpolate<hpp::pinocchio::LieGroupTpl>(device_.lock(), q1, q2, 0.5, qmiddle);
        // p->base(qmiddle);

        // Compute the matrices
        // TODO calls to basisFunctionDerivative could be cached as they do not
        // depend on the inputs.
        p->basisFunctionDerivative(0, 0, coeffs.row(0));
        pinocchio::difference<hpp::pinocchio::LieGroupTpl>(device_.lock(), q1, p->base(), rhs.row(0));
        for (std::size_t i = 0; i < order1.size(); ++i)
          p->basisFunctionDerivative(order1[i], 0, coeffs.row(i+1));
        rhs.middleRows(1, order1.size()).transpose() = derivatives1;

        size_type row = 1 + order1.size();
        p->basisFunctionDerivative(0, 1, coeffs.row(row));
        pinocchio::difference<hpp::pinocchio::LieGroupTpl>(device_.lock(), q2, p->base(), rhs.row(row));
        ++row;
        for (std::size_t i = 0; i < order2.size(); ++i)
          p->basisFunctionDerivative(order2[i], 1, coeffs.row(i+row));
        rhs.middleRows(row, order2.size()).transpose() = derivatives2;

        // Solve the problem
        // coeffs * P = rhs
        typedef Eigen::JacobiSVD < ConstraintMatrix_t > SVD_t;
        SVD_t svd (coeffs, Eigen::ComputeFullU | Eigen::ComputeFullV);
        p->parameters (svd.solve(rhs));

        return p;
      }

      template <int _PB, int _SO>
      Spline<_PB, _SO>::Spline (const Problem& problem) :
        SteeringMethod (problem), device_ (problem.robot ())
      {}

      /// Copy constructor
      template <int _PB, int _SO>
      Spline<_PB, _SO>::Spline (const Spline& other) :
        SteeringMethod (other), device_ (other.device_)
      {}

      template class Spline<path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
      template class Spline<path::CanonicalPolynomeBasis, 2>;
      template class Spline<path::CanonicalPolynomeBasis, 3>;
      template class Spline<path::BernsteinBasis, 1>; // equivalent to StraightPath
      template class Spline<path::BernsteinBasis, 2>;
      template class Spline<path::BernsteinBasis, 3>;
    } // namespace steeringMethod
  } // namespace core
} // namespace hpp
<|endoftext|>


<commit_before>//
//  thread.cpp
//  sutils
//
//  Created by Sandy Martel on 12/03/10.
//  Copyright (c) 2015年 Sandy Martel. All rights reserved.
//
// Permission to use, copy, modify, distribute, and sell this software for any
// purpose is hereby granted without fee. The sotware is provided "AS-IS" and
// without warranty of any kind, express, implied or otherwise.
//

#include "su/concurrency/thread.h"
#include "su/base/platform.h"

#if UPLATFORM_WIN
#include <Windows.h>
#endif

namespace {

std::thread::id g_mainThreadID;

}

namespace su {

namespace this_thread {

void set_as_main()
{
	g_mainThreadID = std::this_thread::get_id();
}

bool is_main()
{
	return g_mainThreadID == std::this_thread::get_id();
}

void set_name( const std::string_view &n )
{
	if ( n.empty() )
		return;

#if UPLATFORM_WIN
	typedef HRESULT(WINAPI* SetThreadDescriptionPtr)( HANDLE, PCWSTR );

	auto SetThreadDescriptionFunc = reinterpret_cast<SetThreadDescriptionPtr>(
							::GetProcAddress( ::GetModuleHandle(L"Kernel32.dll"), "SetThreadDescription" ) );
	if ( SetThreadDescriptionFunc )
	{
		wchar_t name[64];
		int l = MultiByteToWideChar( CP_UTF8, MB_COMPOSITE, n.data(), n.size()), name, 64 );
		if ( l > 0 )
		{
			name[l] = 0;
			SetThreadDescriptionFunc( ::GetCurrentThread(), name );
		}
	}
#elif UPLATFORM_MAC
	std::string name( n );
	pthread_setname_np( name.c_str() );
#else
	std::string name( n );
	if ( name.size() > 15 ) // limited to 16 (including terminator) on linux
		name.resize( 15 );
	pthread_setname_np( pthread_self(), name.c_str() );
#endif
}

}

void set_low_priority( std::thread &i_thread )
{
#if UPLATFORM_WIN
	SetThreadPriority( i_thread.native_handle(), THREAD_PRIORITY_LOWEST );
#else
	sched_param param;
	param.sched_priority = sched_get_priority_min( SCHED_RR );
	::pthread_setschedparam( i_thread.native_handle(), SCHED_RR, &param );
#endif
}

}
<commit_msg>compile error on macos<commit_after>//
//  thread.cpp
//  sutils
//
//  Created by Sandy Martel on 12/03/10.
//  Copyright (c) 2015年 Sandy Martel. All rights reserved.
//
// Permission to use, copy, modify, distribute, and sell this software for any
// purpose is hereby granted without fee. The sotware is provided "AS-IS" and
// without warranty of any kind, express, implied or otherwise.
//

#include "su/concurrency/thread.h"
#include "su/base/platform.h"
#include <string>

#if UPLATFORM_WIN
#include <Windows.h>
#endif

namespace {

std::thread::id g_mainThreadID;

}

namespace su {

namespace this_thread {

void set_as_main()
{
	g_mainThreadID = std::this_thread::get_id();
}

bool is_main()
{
	return g_mainThreadID == std::this_thread::get_id();
}

void set_name( const std::string_view &n )
{
	if ( n.empty() )
		return;

#if UPLATFORM_WIN
	typedef HRESULT(WINAPI* SetThreadDescriptionPtr)( HANDLE, PCWSTR );

	auto SetThreadDescriptionFunc = reinterpret_cast<SetThreadDescriptionPtr>(
							::GetProcAddress( ::GetModuleHandle(L"Kernel32.dll"), "SetThreadDescription" ) );
	if ( SetThreadDescriptionFunc )
	{
		wchar_t name[64];
		int l = MultiByteToWideChar( CP_UTF8, MB_COMPOSITE, n.data(), n.size()), name, 64 );
		if ( l > 0 )
		{
			name[l] = 0;
			SetThreadDescriptionFunc( ::GetCurrentThread(), name );
		}
	}
#elif UPLATFORM_MAC
	std::string name( n );
	pthread_setname_np( name.c_str() );
#else
	std::string name( n );
	if ( name.size() > 15 ) // limited to 16 (including terminator) on linux
		name.resize( 15 );
	pthread_setname_np( pthread_self(), name.c_str() );
#endif
}

}

void set_low_priority( std::thread &i_thread )
{
#if UPLATFORM_WIN
	SetThreadPriority( i_thread.native_handle(), THREAD_PRIORITY_LOWEST );
#else
	sched_param param;
	param.sched_priority = sched_get_priority_min( SCHED_RR );
	::pthread_setschedparam( i_thread.native_handle(), SCHED_RR, &param );
#endif
}

}
<|endoftext|>


<commit_before>// MultLayers.hh for FbTk - fluxbox toolkit
// Copyright (c) 2003 Henrik Kinnunen (fluxgen at users.sourceforge.net)
//                and Simon Bowden    (rathnor at users.sourceforge.net)
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

// $Id: MultLayers.hh,v 1.3 2003/02/02 16:32:41 rathnor Exp $

#ifndef FBTK_MULTLAYERS_HH
#define FBTK_MULTLAYERS_HH

#include <vector>

namespace FbTk {

class XLayerItem;
class XLayer;

class MultLayers {
public:
    MultLayers(int numlayers);
    ~MultLayers();
    XLayerItem *getLowestItemAboveLayer(int layernum);

    // if there are none below, it will return null
    XLayerItem *getItemBelow(XLayerItem &item);
    XLayerItem *getItemAbove(XLayerItem &item);
    void addToTop(XLayerItem &item, int layernum);
    void remove(XLayerItem &item);

    // raise/lower the item a whole layer, not just to top of current layer
    void raise(XLayerItem &item);
    void lower(XLayerItem &item);

    void moveToLayer(XLayerItem &item, int layernum);
    int  size();
    void restack();

private:
    int m_numlayers;
    std::vector<XLayer *> m_layers;

};

};
#endif // FBTK_MULTLAYERS_HH
<commit_msg>removed numlayers<commit_after>// MultLayers.hh for FbTk - fluxbox toolkit
// Copyright (c) 2003 Henrik Kinnunen (fluxgen at users.sourceforge.net)
//                and Simon Bowden    (rathnor at users.sourceforge.net)
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

// $Id: MultLayers.hh,v 1.4 2003/02/03 13:46:42 fluxgen Exp $

#ifndef FBTK_MULTLAYERS_HH
#define FBTK_MULTLAYERS_HH

#include <vector>

namespace FbTk {

class XLayerItem;
class XLayer;

class MultLayers {
public:
    MultLayers(int numlayers);
    ~MultLayers();
    XLayerItem *getLowestItemAboveLayer(int layernum);

    /// if there are none below, it will return null
    XLayerItem *getItemBelow(XLayerItem &item);
    XLayerItem *getItemAbove(XLayerItem &item);
    void addToTop(XLayerItem &item, int layernum);
    void remove(XLayerItem &item);

    // raise/lower the item a whole layer, not just to top of current layer
    void raise(XLayerItem &item);
    void lower(XLayerItem &item);

    void moveToLayer(XLayerItem &item, int layernum);
    int  size();
    void restack();

    XLayer *getLayer(size_t num);
    const XLayer *getLayer(size_t num) const;

private:
    std::vector<XLayer *> m_layers;

};

};
#endif // FBTK_MULTLAYERS_HH
<|endoftext|>


<commit_before>/* -*- mode: C++; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*- */
// vim:sts=8:sw=8:ts=8:noet:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s
/* checkers/Checker_charpoly.inl
 * Copyright (C) 2016 Ashley Lesdalons
 *
 * Written by Ashley Lesdalons <Ashley.Lesdalons@e.ujf-grenoble.fr>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_checker_charpoly_INL
#define __FFLASFFPACK_checker_charpoly_INL

#ifdef ENABLE_CHECKER_charpoly
#include "ffpack/ffpack.h"

#ifdef TIME_CHECKER_CHARPOLY
#include <givaro/givtimer.h>
#endif

namespace FFPACK {
    template <class Field, class Polynomial>
    class Checker_charpoly {

        const Field& F;
        const size_t n;
        typename Field::Element lambda, det;
        bool pass;
#ifdef TIME_CHECKER_CHARPOLY
        Givaro::Timer _time;
#endif

    public:
        Checker_charpoly(const Field& F_, const size_t n_, typename Field::Element_ptr A)
                : F(F_), n(n_)
            {
                typename Field::RandIter G(F);
                init(G,A);
            }

        Checker_charpoly(typename Field::RandIter &G, const size_t n_, typename Field::Element_ptr A)
                : F(G.ring()), n(n_)
            {
                init(G,A);
            }

        ~Checker_charpoly() {
        }

        inline bool check(Polynomial &g) {
#ifdef TIME_CHECKER_CHARPOLY
            Givaro::Timer checktime; checktime.start();
#endif
            typename Field::Element h = F.zero,
                t = F.one,
                u;
            for (size_t i=0; i < g.size(); ++i) {
                F.mul(u,g[i],t);
                F.add(h,h,u);
                F.mul(t,t,lambda);
            }

                // is h == det ?
            pass = pass && F.areEqual(h,det);
            if (!pass) throw FailureCharpolyCheck();

#ifdef TIME_CHECKER_CHARPOLY
            checktime.stop(); _time += checktime;
            std::cerr << "CHARPol CHECK: " << _time << std::endl;
#endif
            return pass;
        }

    private:
        inline void init(typename Field::RandIter &G, typename Field::Element_ptr A) {
#ifdef TIME_CHECKER_CHARPOLY
            Givaro::Timer inittime; inittime.start();
#endif
                // random lambda
            G.random(lambda);

            typename Field::Element_ptr v = FFLAS::fflas_new(F,n,1),
                w = FFLAS::fflas_new(F,n,1),
                Ac = FFLAS::fflas_new(F,n,n);
            FFLAS::frand(F,G,n,v,1);

                // w <- -A.v
            FFLAS::fgemv(F, FFLAS::FflasNoTrans, n, n, F.mOne, A, n, v, 1, F.zero, w, 1);

            if (!F.isZero(lambda)) {
                    // w <- lambda.v + w
                FFLAS::faxpy(F, n, lambda, v, 1, w, 1);
            }

                // Ac <- A - lambda.I
            for (size_t i=0; i<n; ++i)
                for (size_t j=0; j<n; ++j) {
                    if (i==j) F.sub(*(Ac+i*n+i),*(A+i*n+i),lambda);
                    else F.assign(*(Ac+i*n+j),*(A+i*n+j));
                }

                // w <- Ac.v + w
            FFLAS::fgemv(F, FFLAS::FflasNoTrans, n, n, F.one, Ac, n, v, 1, F.one, w, 1);

                // is w == 0 ?
            pass = FFLAS::fiszero(F,n,1,w,1);
            FFLAS::fflas_delete(v,w);
            if (!pass) throw FailureCharpolyCheck();

                // P,Ac,Q <- PLUQ(Ac)
            size_t *P = FFLAS::fflas_new<size_t>(n);
            size_t *Q = FFLAS::fflas_new<size_t>(n);

#ifdef TIME_CHECKER_CHARPOLY
            Givaro::Timer pluqtime; pluqtime.start();
#endif

            FFPACK::PLUQ(F, FFLAS::FflasNonUnit, n, n, Ac, n, P, Q);

#ifdef TIME_CHECKER_CHARPOLY
            pluqtime.stop(); _time -= pluqtime;
            inittime.stop(); _time += inittime;
            std::cerr << "CHARPol server PLUQ:" << pluqtime << std::endl;
            inittime.start();
#endif

                // compute the determinant of A
            F.init(det,*Ac);
            for (size_t i=1; i<n; ++i)
                F.mul(det,det,*(Ac+i*n+i));
            if (n%2 == 1) F.neg(det,det);

                // count the number of permutations
            int t = 0;
            for (size_t i=0; i<n; ++i) {
                if (P[i] != i) t++;
                if (Q[i] != i) t++;
            }
            if (t%2 == 1) F.neg(det,det);

            FFLAS::fflas_delete(Ac);
#ifdef TIME_CHECKER_CHARPOLY
            inittime.stop(); _time += inittime;
#endif
        }
    };

}

#endif // ENABLE_CHECKER_charpoly

#endif // __FFLASFFPACK_checker_charpoly_INL
<commit_msg>replace double loop by fassign<commit_after>/* -*- mode: C++; tab-width: 8; indent-tabs-mode: t; c-basic-offset: 8 -*- */
// vim:sts=8:sw=8:ts=8:noet:sr:cino=>s,f0,{0,g0,(0,\:0,t0,+0,=s
/* checkers/Checker_charpoly.inl
 * Copyright (C) 2016 Ashley Lesdalons
 *
 * Written by Ashley Lesdalons <Ashley.Lesdalons@e.ujf-grenoble.fr>
 *
 *
 * ========LICENCE========
 * This file is part of the library FFLAS-FFPACK.
 *
 * FFLAS-FFPACK is free software: you can redistribute it and/or modify
 * it under the terms of the  GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 * ========LICENCE========
 *.
 */

#ifndef __FFLASFFPACK_checker_charpoly_INL
#define __FFLASFFPACK_checker_charpoly_INL

#ifdef ENABLE_CHECKER_charpoly
#include "ffpack/ffpack.h"

#ifdef TIME_CHECKER_CHARPOLY
#include <givaro/givtimer.h>
#endif

namespace FFPACK {
    template <class Field, class Polynomial>
    class Checker_charpoly {

        const Field& F;
        const size_t n;
        typename Field::Element lambda, det;
        bool pass;
#ifdef TIME_CHECKER_CHARPOLY
        Givaro::Timer _time;
#endif

    public:
        Checker_charpoly(const Field& F_, const size_t n_, typename Field::Element_ptr A)
                : F(F_), n(n_)
            {
                typename Field::RandIter G(F);
                init(G,A);
            }

        Checker_charpoly(typename Field::RandIter &G, const size_t n_, typename Field::Element_ptr A)
                : F(G.ring()), n(n_)
            {
                init(G,A);
            }

        ~Checker_charpoly() {
        }

        inline bool check(Polynomial &g) {
#ifdef TIME_CHECKER_CHARPOLY
            Givaro::Timer checktime; checktime.start();
#endif
            typename Field::Element h = F.zero,
                t = F.one,
                u;
            for (size_t i=0; i < g.size(); ++i) {
                F.mul(u,g[i],t);
                F.add(h,h,u);
                F.mul(t,t,lambda);
            }

                // is h == det ?
            pass = pass && F.areEqual(h,det);
            if (!pass) throw FailureCharpolyCheck();

#ifdef TIME_CHECKER_CHARPOLY
            checktime.stop(); _time += checktime;
            std::cerr << "CHARPol CHECK: " << _time << std::endl;
#endif
            return pass;
        }

    private:
        inline void init(typename Field::RandIter &G, typename Field::Element_ptr A) {
#ifdef TIME_CHECKER_CHARPOLY
            Givaro::Timer inittime; inittime.start();
#endif
                // random lambda
            G.random(lambda);

            typename Field::Element_ptr v = FFLAS::fflas_new(F,n,1),
                w = FFLAS::fflas_new(F,n,1),
                Ac = FFLAS::fflas_new(F,n,n);
            FFLAS::frand(F,G,n,v,1);

                // w <- -A.v
            FFLAS::fgemv(F, FFLAS::FflasNoTrans, n, n, F.mOne, A, n, v, 1, F.zero, w, 1);

            if (!F.isZero(lambda)) {
                    // w <- lambda.v + w
                FFLAS::faxpy(F, n, lambda, v, 1, w, 1);
            }

                // Ac <- A - lambda.I
		// WARNING: should be lda
	    FFLAS::fassign(F,n,n,A,n,Ac,n);
            for (size_t i=0; i<n; ++i)
		    F.sub(*(Ac+i*n+i),*(A+i*n+i),lambda);

                // w <- Ac.v + w
            FFLAS::fgemv(F, FFLAS::FflasNoTrans, n, n, F.one, Ac, n, v, 1, F.one, w, 1);

                // is w == 0 ?
            pass = FFLAS::fiszero(F,n,1,w,1);
            FFLAS::fflas_delete(v,w);
            if (!pass) throw FailureCharpolyCheck();

                // P,Ac,Q <- PLUQ(Ac)
            size_t *P = FFLAS::fflas_new<size_t>(n);
            size_t *Q = FFLAS::fflas_new<size_t>(n);

#ifdef TIME_CHECKER_CHARPOLY
            Givaro::Timer pluqtime; pluqtime.start();
#endif

            FFPACK::PLUQ(F, FFLAS::FflasNonUnit, n, n, Ac, n, P, Q);

#ifdef TIME_CHECKER_CHARPOLY
            pluqtime.stop(); _time -= pluqtime;
            inittime.stop(); _time += inittime;
            std::cerr << "CHARPol server PLUQ:" << pluqtime << std::endl;
            inittime.start();
#endif

                // compute the determinant of A
            F.init(det,*Ac);
            for (size_t i=1; i<n; ++i)
                F.mul(det,det,*(Ac+i*n+i));
            if (n%2 == 1) F.neg(det,det);

                // count the number of permutations
            int t = 0;
            for (size_t i=0; i<n; ++i) {
                if (P[i] != i) t++;
                if (Q[i] != i) t++;
            }
            if (t%2 == 1) F.neg(det,det);

            FFLAS::fflas_delete(Ac);
#ifdef TIME_CHECKER_CHARPOLY
            inittime.stop(); _time += inittime;
#endif
        }
    };

}

#endif // ENABLE_CHECKER_charpoly

#endif // __FFLASFFPACK_checker_charpoly_INL
<|endoftext|>