// TODO: Merge the functions below with aol::RGBColorMap
void EpsWriter::setTopographical256 ( aol::Vector<float>* red, aol::Vector<float>* green, aol::Vector<float>* blue ) {
aol::Vector<float> redv ( 12 ), greenv ( 12 ), bluev ( 12 );
redv[ 0] = 0.0; greenv[ 0] = 0.0; bluev [ 0] = 0.5;
redv[ 1] = 0.0; greenv[ 1] = 0.0; bluev [ 1] = 1.0;
redv[ 2] = 0.4; greenv[ 2] = 0.4; bluev [ 2] = 1.0;
redv[ 3] = 0.6; greenv[ 3] = 0.6; bluev [ 3] = 1.0;
redv[ 4] = 0.3; greenv[ 4] = 1.0; bluev [ 4] = 0.3;
redv[ 5] = 0.0; greenv[ 5] = 1.0; bluev [ 5] = 0.0;
redv[ 6] = 0.0; greenv[ 6] = 0.5; bluev [ 6] = 0.0;
redv[ 7] = 0.3; greenv[ 7] = 0.2; bluev [ 7] = 0.0;
redv[ 8] = 0.5; greenv[ 8] = 0.3; bluev [ 8] = 0.0;
redv[ 9] = 0.8; greenv[ 9] = 0.5; bluev [ 9] = 0.0;
redv[10] = 1.0; greenv[10] = 0.7; bluev [10] = 0.5;
redv[11] = 1.0; greenv[11] = 0.85; bluev [11] = 0.75;
red->resize ( 256 );
green->resize ( 256 );
blue->resize ( 256 );
for ( int i = 0; i < 256; i++ ) {
red->set ( i, redv.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
green->set ( i, greenv.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
blue->set ( i, bluev.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
}
}
visualization_msgs::MarkerArray viz::getAxisMarkerArray(const std::vector<double> &pose, double length, std::string frame_id, std::string ns, int id)
{
visualization_msgs::MarkerArray ma;
std::vector<double> redv(6,0), bluev(6,0), greenv(6,0), arrow_pose(6,0);
redv[0] = length; greenv[1] = length; bluev[2] = length;
leatherman::multiply(pose, redv, arrow_pose);
ma.markers.push_back(getArrowMarker(pose, arrow_pose, length/10.0, length/5.0, 0, frame_id, ns, id));
leatherman::multiply(pose, greenv, arrow_pose);
ma.markers.push_back(getArrowMarker(pose, arrow_pose, length/10.0, length/5.0, 120, frame_id, ns, id+1));
leatherman::multiply(pose, bluev, arrow_pose);
ma.markers.push_back(getArrowMarker(pose, arrow_pose, length/10.0, length/5.0, 240, frame_id, ns, id+2));
return ma;
}
void EpsWriter::setWhiteToBlack256 ( aol::Vector<float>* red, aol::Vector<float>* green, aol::Vector<float>* blue ) {
aol::Vector<float> redv ( 2 ), greenv ( 2 ), bluev ( 2 );
// greyscale
redv[ 0] = 1.0; greenv[ 0] = 1.0; bluev [ 0] = 1.0; // white
redv[ 1] = 0.0; greenv[ 1] = 0.0; bluev [ 1] = 0.0; // black
red->resize ( 256 );
green->resize ( 256 );
blue->resize ( 256 );
for ( int i = 0; i < 256; i++ ) {
red->set ( i, redv.interpolateInRange ( i / 255.0f, 0.0f, 1.0f ) );
green->set ( i, greenv.interpolateInRange ( i / 255.0f, 0.0f, 1.0f ) );
blue->set ( i, bluev.interpolateInRange ( i / 255.0f, 0.0f, 1.0f ) );
}
}
void EpsWriter::setBlueToRed256 ( aol::Vector<float>* red, aol::Vector<float>* green, aol::Vector<float>* blue ) {
aol::Vector<float> redv ( 4 ), greenv ( 4 ), bluev ( 4 );
redv[ 0] = 0.0; greenv[ 0] = 0.0; bluev [ 0] = 1.0;
redv[ 1] = 0.4; greenv[ 1] = 0.0; bluev [ 1] = 0.6;
redv[ 2] = 0.6; greenv[ 2] = 0.0; bluev [ 2] = 0.4;
redv[ 3] = 1.0; greenv[ 3] = 0.0; bluev [ 3] = 0.0;
red->resize ( 256 );
green->resize ( 256 );
blue->resize ( 256 );
for ( int i = 0; i < 256; i++ ) {
red->set ( i, redv.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
green->set ( i, greenv.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
blue->set ( i, bluev.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
}
}
//equal to HSV_BLUE_TO_RED
void EpsWriter::setColortrans256 ( aol::Vector<float>* red, aol::Vector<float>* green, aol::Vector<float>* blue ) {
aol::Vector<float> redv ( 5 ), greenv ( 5 ), bluev ( 5 );
// hsv blue to red
redv[ 0] = 0.0; greenv[ 0] = 0.0; bluev [ 0] = 1.0; // blue
redv[ 1] = 0.0; greenv[ 1] = 1.0; bluev [ 1] = 1.0; // cyan
redv[ 2] = 0.0; greenv[ 2] = 1.0; bluev [ 2] = 0.0; // green
redv[ 3] = 1.0; greenv[ 3] = 1.0; bluev [ 3] = 0.0; // yellow
redv[ 4] = 1.0; greenv[ 4] = 0.0; bluev [ 4] = 0.0; // red
red->resize ( 256 );
green->resize ( 256 );
blue->resize ( 256 );
for ( int i = 0; i < 256; i++ ) {
red->set ( i, redv.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
green->set ( i, greenv.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
blue->set ( i, bluev.interpolateInRange ( i / 255.0f, -1.0f, 1.0f ) );
}
}
