void UniformResamplingRecursiveMISBPTRenderer::connectLightVerticesToSensor(uint32_t threadID, uint32_t tileID, const Vec4u& viewport) const {
auto rcpPathCount = 1.f / m_nLightPathCount;
auto mis = [&](float v) {
return Mis(v);
};
for(const auto& directImportanceSample: m_DirectImportanceSampleTilePartitionning[tileID]) {
auto pLightVertex = &m_LightPathBuffer[directImportanceSample.m_nLightVertexIndex];
auto totalDepth = 1 + pLightVertex->m_nDepth;
if(!acceptPathDepth(totalDepth) || totalDepth > m_nMaxDepth) {
continue;
}
auto pixel = directImportanceSample.m_Pixel;
auto pixelID = BnZ::getPixelIndex(pixel, getFramebufferSize());
PixelSensor sensor(getSensor(), pixel, getFramebufferSize());
auto contrib = rcpPathCount * connectVertices(*pLightVertex,
SensorVertex(&sensor, directImportanceSample.m_LensSample),
getScene(), m_nLightPathCount, mis);
if(isInvalidMeasurementEstimate(contrib)) {
reportInvalidContrib(threadID, tileID, pixelID, [&]() {
debugLog() << "s = " << (pLightVertex->m_nDepth + 1) << ", t = " << 1 << std::endl;
});
}
accumulate(FINAL_RENDER, pixelID, Vec4f(contrib, 0.f));
accumulate(FINAL_RENDER_DEPTH1 + totalDepth - 1u, pixelID, Vec4f(contrib, 0.f));
auto strategyOffset = computeBPTStrategyOffset(totalDepth + 1, pLightVertex->m_nDepth + 1);
accumulate(BPT_STRATEGY_s0_t2 + strategyOffset, pixelID, Vec4f(contrib, 0));
}
}
void Explorer::loadSensors()
{
qDebug() << "Explorer::loadSensors";
// Clear out anything that's in there now
ui.sensors->clear();
foreach (const QByteArray &type, QSensor::sensorTypes()) {
qDebug() << "Found type" << type;
foreach (const QByteArray &identifier, QSensor::sensorsForType(type)) {
qDebug() << "Found identifier" << identifier;
// Don't put in sensors we can't connect to
QSensor sensor(type);
sensor.setIdentifier(identifier);
if (!sensor.connectToBackend()) {
qDebug() << "Couldn't connect to" << identifier;
continue;
}
qDebug() << "Adding identifier" << identifier;
QTreeWidgetItem *item = new QTreeWidgetItem(QStringList() << QString::fromLatin1(identifier));
item->setData(0, Qt::UserRole, QString::fromLatin1(type));
ui.sensors->addTopLevelItem(item);
}
bool NounShip::canDetect( Noun * pNoun ) const
{
return NounGame::canDetect( pNoun, sensor(), view() );
}
int SavePCD::compute()
{
ccPointCloud * cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//search for a sensor as child
size_t n_childs = cloud->getChildrenNumber();
ccSensor * sensor(0);
for (size_t i = 0; i < n_childs; ++i)
{
ccHObject * child = cloud->getChild(i);
//try to cast to a ccSensor
if (!child->isKindOf(CC_TYPES::SENSOR))
continue;
sensor = ccHObjectCaster::ToSensor(child);
}
PCLCloud::Ptr out_cloud (new PCLCloud);
cc2smReader* converter = new cc2smReader();
converter->setInputCloud(cloud);
int result = converter->getAsSM(*out_cloud);
delete converter;
converter=0;
Eigen::Vector4f pos;
Eigen::Quaternionf ori;
if(!sensor)
{
//we append to the cloud null sensor informations
pos = Eigen::Vector4f::Zero ();
ori = Eigen::Quaternionf::Identity ();
}
else
{
//we get out valid sensor informations
ccGLMatrix mat = sensor->getRigidTransformation();
CCVector3 trans = mat.getTranslationAsVec3D();
pos(0) = trans[0];
pos(1) = trans[1];
pos(2) = trans[2];
//also the rotation
Eigen::Matrix3f eigrot;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
eigrot(i,j) = mat.getColumn(j)[i];
// now translate to a quaternion notation
ori = Eigen::Quaternionf(eigrot);
}
if (result != 1)
{
return -31;
}
if (pcl::io::savePCDFile( m_filename.toStdString(), *out_cloud, pos, ori, true) < 0)
{
return -32;
}
return 1;
}
bool SensorfwSensorBase::initSensorInterface(QString const &name)
{
if (!m_sensorInterface) {
sensorError(KErrNotFound);
return false;
}
//metadata
const QList<DataRange> intervals = m_sensorInterface->getAvailableIntervals();
for (int i = 0, l = intervals.size(); i < l; i++) {
qreal intervalMax = intervals.at(i).max;
qreal intervalMin = intervals.at(i).min;
if (intervalMin == 0 && intervalMax == 0) {
// 0 interval has different meanings in e.g. magge/acce
// magge -> best-effort
// acce -> lowest possible
// in Qt API setting 0 means default
continue;
}
qreal rateMin = intervalMax < 1 ? 1 : 1 / intervalMax * 1000;
rateMin = rateMin < 1 ? 1 : rateMin;
intervalMin = intervalMin < 1 ? 10: intervalMin; // do not divide with 0
qreal rateMax = 1 / intervalMin * 1000;
addDataRate(rateMin, rateMax);
}
//bufferSizes
if (m_bufferingSensors.contains(sensor()->identifier())) {
IntegerRangeList sizes = m_sensorInterface->getAvailableBufferSizes();
for (int i = 0; i < sizes.size(); i++) {
int second = sizes.at(i).second;
m_maxBufferSize = second > m_bufferSize ? second : m_maxBufferSize;
}
m_maxBufferSize = m_maxBufferSize < 0 ? 1 : m_maxBufferSize;
//SensorFW guarantees to provide the most efficient size first
//TODO: remove from comments
//m_efficientBufferSize = m_sensorInterface->hwBuffering()? (l>0?sizes.at(0).first:1) : 1;
} else {
m_maxBufferSize = 1;
}
sensor()->setMaxBufferSize(m_maxBufferSize);
sensor()->setEfficientBufferSize(m_efficientBufferSize);
// TODO deztructor: Leaking abstraction detected. Just copied code
// from initSensor<>() here, need to
QByteArray type = sensor()->type();
if ((type == QAmbientLightSensor::type) // SensorFW returns lux values, plugin enumerated values
|| (type == QIRProximitySensor::type) // SensorFW returns raw reflectance values, plugin % of max reflectance
|| (name == "accelerometersensor") // SensorFW returns milliGs, plugin m/s^2
|| (name == "magnetometersensor") // SensorFW returns nanoTeslas, plugin Teslas
|| (name == "gyroscopesensor")) // SensorFW returns DSPs, plugin milliDSPs
return true;
setDescription(m_sensorInterface->description());
if (name == "tapsensor") return true;
setRanges();
return true;
}
int main(int argc, char* argv[]){
CMDParser p("basefilename_cv .... serverport");
p.addOpt("bbx",6,"bounding_box", "specify the bounding box x_min y_min z_min x_max y_max z_max in meters, default -10.0 -10.0 -10.0 10.0 10.0 10.0");
p.init(argc,argv);
if(p.isOptSet("bbx")){
bbx_min = glm::vec3(p.getOptsFloat("bbx")[0], p.getOptsFloat("bbx")[1], p.getOptsFloat("bbx")[2]);
bbx_max = glm::vec3(p.getOptsFloat("bbx")[3], p.getOptsFloat("bbx")[4], p.getOptsFloat("bbx")[5]);
std::cout << "setting bounding box to min: " << bbx_min << " -> max: " << bbx_max << std::endl;
}
const unsigned num_streams(p.getArgs().size() - 1);
std::vector<CalibVolume*> cvs;
for(unsigned i = 0; i < num_streams; ++i){
std::string basefilename = p.getArgs()[i];
std::string filename_xyz(basefilename + "_xyz");
std::string filename_uv(basefilename + "_uv");
cvs.push_back(new CalibVolume(filename_xyz.c_str(), filename_uv.c_str()));
}
RGBDConfig cfg;
cfg.serverport = p.getArgs()[num_streams];
cfg.size_rgb = glm::uvec2(1280, 1080);
cfg.size_d = glm::uvec2(512, 424);
RGBDSensor sensor(cfg, num_streams - 1);
Window win(glm::ivec2(800,800), true /*3D mode*/);
while (!win.shouldClose()) {
auto t = win.getTime();
if (win.isKeyPressed(GLFW_KEY_ESCAPE)) {
win.stop();
}
// receive frames
sensor.recv(false /*do not recv ir!*/);
sensor.display_rgb_d();
std::vector<glm::vec3> candidate_positions;
float max_y_level = std::numeric_limits<float>::lowest();
for(unsigned s_num = 0; s_num < num_streams; ++s_num){
// do 3D recosntruction for each depth pixel
for(unsigned y = 0; y < sensor.config.size_d.y; ++y){
for(unsigned x = 0; x < sensor.config.size_d.x; ++x){
const unsigned d_idx = y* sensor.config.size_d.x + x;
float d = s_num == 0 ? sensor.frame_d[d_idx] : sensor.slave_frames_d[s_num - 1][d_idx];
if(d < cvs[s_num]->min_d || d > cvs[s_num]->max_d)
continue;
glm::vec3 pos3D = cvs[s_num]->lookupPos3D( x * 1.0/sensor.config.size_d.x,
y * 1.0/sensor.config.size_d.y, d);
if(clip(pos3D)){
continue;
}
max_y_level = std::max(max_y_level, pos3D.y);
candidate_positions.push_back(pos3D);
}
}
}
const float head_height = 0.3;
std::vector<glm::vec3> head_candidate_positions;
for(const auto& p : candidate_positions){
if(max_y_level - p.y < head_height){
head_candidate_positions.push_back(p);
}
}
glDisable(GL_DEPTH_TEST);
glBegin(GL_POINTS);
glm::vec3 avg(0.0,0.0,0.0);
for(const auto& p : head_candidate_positions){
avg.x += p.x;
avg.y += p.y;
avg.z += p.z;
glColor3f(0.0,0.0,1.0);
glVertex3f(p.x, p.y, p.z);
}
avg.x /= head_candidate_positions.size();
avg.y /= head_candidate_positions.size();
avg.z /= head_candidate_positions.size();
glPointSize(5.0);
glColor3f(1.0,0.0,0.0);
glVertex3f(avg.x, avg.y, avg.z);
glEnd();
win.update();
}
return 0;
}
float GadgetScanner::addSensor() const
{
return active() ? (damageRatioInv() * sensor() * calculateModifier( MT_SCANNER ) ) : 0.0f;
}
bool QTQmlSensor::skipDuplicates() const
{
return sensor()->skipDuplicates();
}
QString QTQmlSensor::type() const
{
return QString::fromLatin1(sensor()->type());
}
bool QTQmlSensor::isConnectedToBackend() const
{
return sensor()->isConnectedToBackend();
}
bool QTQmlSensor::isAlwaysOn() const
{
return sensor()->isAlwaysOn();
}
int dev_lst_cmd_parse(FILE * f, int argc, char ** argv,
uint32_t sb[], uint32_t mb[])
{
struct ss_device * dev;
bool sens = false;
bool mod = false;
bool all = false;
bool grp = false;
unsigned int addr;
int k;
int i;
memset(sb, 0, 160 / 8);
memset(mb, 0, 160 / 8);
if ((argc == 2) && (strcmp(argv[1], "all") == 0)) {
all = true;
mod = true;
sens = true;
} else {
if (argc < 3)
return SHELL_ERR_ARG_MISSING;
if ((strcmp(argv[1], "sens") == 0) ||
(strcmp(argv[1], "s") == 0)) {
sens = true;
} else if ((strcmp(argv[1], "mod") == 0) ||
(strcmp(argv[1], "m") == 0)) {
mod = true;
} else if ((strcmp(argv[1], "grp") == 0) ||
(strcmp(argv[1], "g") == 0)) {
grp = true;
} else {
return SHELL_ERR_ARG_INVALID;
}
if ((argc == 3) && (strcmp(argv[2], "all") == 0))
all = true;
}
if (all & !grp) {
if (sens) {
for (i = 0; i < (160 / 32); ++i)
sb[i] = 0xffffffff;
fprintf(f, " All sensors\n");
}
if (mod) {
for (i = 0; i < (160 / 32); ++i)
mb[i] = 0xffffffff;
fprintf(f, " All modules\n");
}
return 0;
}
if (grp) {
if (all) {
for (addr = 0; addr < 160; ++addr) {
dev = sensor(addr);
if ((dev->grp[0] != 0) || (dev->grp[1] != 0) ||
(dev->grp[2] != 0) || (dev->grp[3] != 0))
sb[addr / 32] |= 1 << (addr % 32);
dev = module(addr);
if ((dev->grp[0] != 0) || (dev->grp[1] != 0) ||
(dev->grp[2] != 0) || (dev->grp[3] != 0))
mb[addr / 32] |= 1 << (addr % 32);
}
fprintf(f, " All groups\n");
} else {
for (k = 2; k < argc; ++k) {
unsigned int g = strtoul(argv[k], NULL, 0);
if ((g < 1) || (g > 256))
return SHELL_ERR_ARG_INVALID;
for (addr = 0; addr < 160; ++addr) {
dev = sensor(addr);
if ((dev->grp[0] == g) || (dev->grp[1] == g) ||
(dev->grp[2] == g) || (dev->grp[3] == g))
sb[addr / 32] |= 1 << (addr % 32);
dev = module(addr);
if ((dev->grp[0] == g) || (dev->grp[1] == g) ||
(dev->grp[2] == g) || (dev->grp[3] == g))
mb[addr / 32] |= 1 << (addr % 32);
}
fprintf(f, " Group %d\n", g);
}
}
return 0;
}
for (k = 2; k < argc; ++k) {
addr = strtoul(argv[k], NULL, 0);
if ((addr < 0) || (addr > 159))
return SHELL_ERR_ARG_INVALID;
if (sens) {
sb[addr / 32] |= (1 << (addr % 32));
fprintf(f, " Sensor %d\n", addr);
}
if (mod) {
mb[addr / 32] |= (1 << (addr % 32));
fprintf(f, " Module %d\n", addr);
}
}
return 0;
}
bool GenericTiltSensor::filter(QAccelerometerReading *reading)
{
/*
z y
| /
|/___ x
*/
qreal ax = reading->x();
qreal ay = reading->y();
qreal az = reading->z();
#ifdef LOGCALIBRATION
qDebug() << "------------ new value -----------";
qDebug() << "old _pitch: " << pitch;
qDebug() << "old _roll: " << roll;
qDebug() << "_calibratedPitch: " << calibratedPitch;
qDebug() << "_calibratedRoll: " << calibratedRoll;
#endif
pitch = calcPitch(ax, ay, az);
roll = calcRoll (ax, ay, az);
#ifdef LOGCALIBRATION
qDebug() << "_pitch: " << pitch;
qDebug() << "_roll: " << roll;
#endif
qreal xrot = roll - calibratedRoll;
qreal yrot = pitch - calibratedPitch;
//get angle beteen 0 and 180 or 0 -180
qreal aG = 1 * sin(xrot);
qreal aK = 1 * cos(xrot);
xrot = qAtan2(aG, aK);
if (xrot > M_PI_2)
xrot = M_PI - xrot;
else if (xrot < -M_PI_2)
xrot = -(M_PI + xrot);
aG = 1 * sin(yrot);
aK = 1 * cos(yrot);
yrot = qAtan2(aG, aK);
if (yrot > M_PI_2)
yrot = M_PI - yrot;
else if (yrot < -M_PI_2)
yrot = -(M_PI + yrot);
#ifdef LOGCALIBRATION
qDebug() << "new xrot: " << xrot;
qDebug() << "new yrot: " << yrot;
qDebug() << "----------------------------------";
#endif
qreal dxrot = rad2deg(xrot) - xRotation;
qreal dyrot = rad2deg(yrot) - yRotation;
if (dxrot < 0) dxrot = -dxrot;
if (dyrot < 0) dyrot = -dyrot;
bool setNewReading = false;
if (dxrot >= rad2deg(radAccuracy) || !sensor()->skipDuplicates()) {
xRotation = rad2deg(xrot);
setNewReading = true;
}
if (dyrot >= rad2deg(radAccuracy) || !sensor()->skipDuplicates()) {
yRotation = rad2deg(yrot);
setNewReading = true;
}
if (setNewReading || m_reading.timestamp() == 0) {
m_reading.setTimestamp(reading->timestamp());
m_reading.setXRotation(xRotation);
m_reading.setYRotation(yRotation);
newReadingAvailable();
}
return false;
}
int main(int argc, char* argv[]){
unsigned num_threads = 16;
bool rgb_is_compressed = false;
std::string stream_filename;
CMDParser p("basefilename_cv .... basefilename_for_output");
p.addOpt("s",1,"stream_filename", "specify the stream filename which should be converted");
p.addOpt("n",1,"num_threads", "specify how many threads should be used, default 16");
p.addOpt("c",-1,"rgb_is_compressed", "enable compressed support for rgb stream, default: false (not compressed)");
p.addOpt("p1d",1,"filter_pass_1_max_avg_dist_in_meter", "filter pass 1 skips points which have an average distance of more than this to it k neighbors, default 0.025");
p.addOpt("p1k",1,"filter_pass_1_k", "filter pass 1 number of neighbors, default 50");
p.addOpt("p2s",1,"filter_pass_2_sd_fac", "filter pass 2, specify how many times a point's distance should be above (values higher than 1.0) / below (values smaller than 1.0) is allowed to be compared to standard deviation of its k neighbors, default 1.0");
p.addOpt("p2k",1,"filter_pass_2_k", "filter pass 2 number of neighbors (the higher the more to process) , default 50");
p.addOpt("bbx",6,"bounding_box", "specify the bounding box x_min y_min z_min x_max y_max z_max in meters, default -1.2 -0.05 -1.2 1.2 2.4 1.2");
p.init(argc,argv);
if(p.isOptSet("bbx")){
bbx_min = glm::vec3(p.getOptsFloat("bbx")[0], p.getOptsFloat("bbx")[1], p.getOptsFloat("bbx")[2]);
bbx_max = glm::vec3(p.getOptsFloat("bbx")[3], p.getOptsFloat("bbx")[4], p.getOptsFloat("bbx")[5]);
std::cout << "setting bounding box to min: " << bbx_min << " -> max: " << bbx_max << std::endl;
}
if(p.isOptSet("p1d")){
filter_pass_1_max_avg_dist_in_meter = p.getOptsFloat("p1d")[0];
std::cout << "setting filter_pass_1_max_avg_dist_in_meter to " << filter_pass_1_max_avg_dist_in_meter << std::endl;
}
if(p.isOptSet("p1k")){
filter_pass_1_k = p.getOptsInt("p1k")[0];
std::cout << "setting filter_pass_1_k to " << filter_pass_1_k << std::endl;
}
if(p.isOptSet("p2s")){
filter_pass_2_sd_fac = p.getOptsFloat("p2s")[0];
std::cout << "setting filter_pass_2_sd_fac to " << filter_pass_2_sd_fac << std::endl;
}
if(p.isOptSet("p2k")){
filter_pass_2_k = p.getOptsInt("p2k")[0];
std::cout << "setting filter_pass_2_k to " << filter_pass_2_k << std::endl;
}
if(p.isOptSet("s")){
stream_filename = p.getOptsString("s")[0];
}
else{
std::cerr << "ERROR, please specify stream filename with flag -s, see " << argv[0] << " -h for help" << std::endl;
return 0;
}
if(p.isOptSet("n")){
num_threads = p.getOptsInt("n")[0];
}
if(p.isOptSet("c")){
rgb_is_compressed = true;
}
const unsigned num_streams(p.getArgs().size() - 1);
const std::string basefilename_for_output = p.getArgs()[num_streams];
std::vector<CalibVolume*> cvs;
for(unsigned i = 0; i < num_streams; ++i){
std::string basefilename = p.getArgs()[i];
std::string filename_xyz(basefilename + "_xyz");
std::string filename_uv(basefilename + "_uv");
cvs.push_back(new CalibVolume(filename_xyz.c_str(), filename_uv.c_str()));
}
RGBDConfig cfg;
cfg.size_rgb = glm::uvec2(1280, 1080);
cfg.size_d = glm::uvec2(512, 424);
RGBDSensor sensor(cfg, num_streams - 1);
unsigned char* tmp_rgb = 0;
unsigned char* tmp_rgba = 0;
const unsigned colorsize_tmp = cfg.size_rgb.x * cfg.size_rgb.y * 3;
const unsigned colorsize_tmpa = cfg.size_rgb.x * cfg.size_rgb.y * 4;
if(rgb_is_compressed){
tmp_rgb = new unsigned char [colorsize_tmp];
tmp_rgba = new unsigned char [colorsize_tmpa];
}
const unsigned colorsize = rgb_is_compressed ? 691200 : cfg.size_rgb.x * cfg.size_rgb.y * 3;
const unsigned depthsize = cfg.size_d.x * cfg.size_d.y * sizeof(float);
FileBuffer fb(stream_filename.c_str());
if(!fb.open("r")){
std::cerr << "ERROR, while opening " << stream_filename << " exiting..." << std::endl;
return 1;
}
const unsigned num_frames = fb.calcNumFrames(num_streams * (colorsize + depthsize));
double curr_frame_time = 0.0;
unsigned frame_num = 0;
while(frame_num < num_frames){
++frame_num;
for(unsigned s_num = 0; s_num < num_streams; ++s_num){
fb.read((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), colorsize);
if(s_num == 0){
memcpy((char*) &curr_frame_time, (const char*) sensor.frame_rgb, sizeof(double));
std::cout << "curr_frame_time: " << curr_frame_time << std::endl;
}
if(rgb_is_compressed){
// uncompress to rgb_tmp from (unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]) to tmp_rgba
squish::DecompressImage (tmp_rgba, cfg.size_rgb.x, cfg.size_rgb.y,
(unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), squish::kDxt1);
// copy back rgbsensor
unsigned buffida = 0;
unsigned buffid = 0;
for(unsigned y = 0; y < cfg.size_rgb.y; ++y){
for(unsigned x = 0; x < cfg.size_rgb.x; ++x){
tmp_rgb[buffid++] = tmp_rgba[buffida++];
tmp_rgb[buffid++] = tmp_rgba[buffida++];
tmp_rgb[buffid++] = tmp_rgba[buffida++];
buffida++;
}
}
memcpy((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), tmp_rgb, colorsize_tmp);
}
fb.read((unsigned char*) (s_num == 0 ? sensor.frame_d : sensor.slave_frames_d[s_num - 1]), depthsize);
}
std::vector<nniSample> nnisamples;
for(unsigned s_num = 0; s_num < num_streams; ++s_num){
// do 3D reconstruction for each depth pixel
for(unsigned y = 0; y < sensor.config.size_d.y; ++y){
for(unsigned x = 0; x < (sensor.config.size_d.x - 3); ++x){
const unsigned d_idx = y* sensor.config.size_d.x + x;
float d = s_num == 0 ? sensor.frame_d[d_idx] : sensor.slave_frames_d[s_num - 1][d_idx];
if(d < cvs[s_num]->min_d || d > cvs[s_num]->max_d){
continue;
}
glm::vec3 pos3D;
glm::vec2 pos2D_rgb;
pos3D = cvs[s_num]->lookupPos3D( x * 1.0/sensor.config.size_d.x,
y * 1.0/sensor.config.size_d.y, d);
if(clip(pos3D)){
continue;
}
nniSample nnis;
nnis.s_pos.x = pos3D.x;
nnis.s_pos.y = pos3D.y;
nnis.s_pos.z = pos3D.z;
glm::vec2 pos2D_rgb_norm = cvs[s_num]->lookupPos2D_normalized( x * 1.0/sensor.config.size_d.x,
y * 1.0/sensor.config.size_d.y, d);
pos2D_rgb = glm::vec2(pos2D_rgb_norm.x * sensor.config.size_rgb.x,
pos2D_rgb_norm.y * sensor.config.size_rgb.y);
glm::vec3 rgb = sensor.get_rgb_bilinear_normalized(pos2D_rgb, s_num);
nnis.s_pos_off.x = rgb.x;
nnis.s_pos_off.y = rgb.y;
nnis.s_pos_off.z = rgb.z;
nnisamples.push_back(nnis);
}
}
}
std::cout << "start building acceleration structure for filtering " << nnisamples.size() << " points..." << std::endl;
NearestNeighbourSearch nns(nnisamples);
std::vector<std::vector<nniSample> > results;
for(unsigned tid = 0; tid < num_threads; ++tid){
results.push_back(std::vector<nniSample>() );
}
std::cout << "start filtering frame " << frame_num << " using " << num_threads << " threads." << std::endl;
boost::thread_group threadGroup;
for (unsigned tid = 0; tid < num_threads; ++tid){
threadGroup.create_thread(boost::bind(&filterPerThread, &nns, &nnisamples, &results, tid, num_threads));
}
threadGroup.join_all();
#if 0
unsigned tid = 0;
for(unsigned sid = 0; sid < nnisamples.size(); ++sid){
nniSample s = nnisamples[sid];
std::vector<nniSample> neighbours = nns.search(s,filter_pass_1_k);
if(neighbours.empty()){
continue;
}
const float avd = calcAvgDist(neighbours, s);
if(avd > filter_pass_1_max_avg_dist_in_meter){
continue;
}
std::vector<float> dists;
for(const auto& n : neighbours){
std::vector<nniSample> local_neighbours = nns.search(n,filter_pass_2_k);
if(!local_neighbours.empty()){
dists.push_back(calcAvgDist(local_neighbours, n));
}
}
double mean;
double sd;
calcMeanSD(dists, mean, sd);
if((avd - mean) > filter_pass_2_sd_fac * sd){
continue;
}
results[tid].push_back(s);
}
#endif
const std::string pcfile_name(basefilename_for_output + "_" + toStringP(frame_num, 5 /*fill*/) + ".xyz");
std::ofstream pcfile(pcfile_name.c_str());
std::cout << "start writing to file " << pcfile_name << " ..." << std::endl;
for(unsigned tid = 0; tid < num_threads; ++tid){
for(const auto& s : results[tid]){
int red = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.x * 255.0f));
int green = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.y * 255.0f));
int blue = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.z * 255.0f));
pcfile << s.s_pos.x << " " << s.s_pos.y << " " << s.s_pos.z << " "
<< red << " "
<< green << " "
<< blue << std::endl;
}
}
pcfile.close();
const std::string tsfile_name(basefilename_for_output + "_" + toStringP(frame_num, 5 /*fill*/) + ".timestamp");
std::ofstream tsfile(tsfile_name.c_str());
tsfile << curr_frame_time << std::endl;
tsfile.close();
std::cout << frame_num
<< " from "
<< num_frames
<< " processed and saved to: "
<< pcfile_name << std::endl << std::endl;
}
return 0;
}
int main(int argc, const char *argv[])
{
// Initialize a connection to MySQL
MYSQL *con = mysql_init(NULL);
if(con == NULL)
{
error_exit(con);
}
// Connect to MySQL
// Here we pass in:
// host name => localhost
// user name => bone
// password => bone
// database name > TempDB
if(mysql_real_connect(con,"localhost","bone","bone","TempDB",0,NULL,0) == NULL)
{
error_exit(con);
}
// Create the Conductivity Measurement database (if it doesn't exist already)
if(mysql_query(con, "CREATE TABLE IF NOT EXISTS ConducMeasure(MeasureTime DATETIME, Conductivity DOUBLE)"))
{
error_exit(con);
}
// Initialize a MySQL statement
MYSQL_STMT *stmt = mysql_stmt_init(con);
if(stmt == NULL)
{
error_exit(con);
}
// Set out insert query as the MySQL statement
const char *query = "INSERT INTO ConducMeasure(MeasureTime, Conductivity) VALUES(NOW(), ?)";
if(mysql_stmt_prepare(stmt, query, strlen(query)))
{
error_exit(con);
}
// Create the MySQL bind structure to store the data that we are going to insert
double conductivity = 0.0;
MYSQL_BIND bind;
memset(&bind, 0, sizeof(bind));
bind.buffer_type = MYSQL_TYPE_DOUBLE;
bind.buffer = (char *)&conductivity;
bind.buffer_length = sizeof(double);
// Bind the data structure to the MySQL statement
if (mysql_stmt_bind_param(stmt, &bind))
{
error_exit(con);
}
// Initialiaze Conductivity Sensor
//CONDUCTIVITY sensor("/sys/devices/ocp.2/helper.14/AIN4");
CONDUCTIVITY sensor("/sys/devices/ocp.3/helper.15/AIN4");
// read Conductivity and insert into database
conductivity = sensor.GetConductivity();
printf("%f\n", conductivity);
mysql_stmt_execute(stmt);
// Insert multiple records into the database with different data each time
//for(int i = 0; i < 1; i++)
for(;;)
{
conductivity = sensor.GetConductivity();
printf("%f\n", conductivity);
mysql_stmt_execute(stmt);
sleep(5);
}
// Close the conductivity sensor
sensor.Close();
// Close the MySQL connection
mysql_close(con);
return conductivity;
}
/**
* Node entry-point. Handles ROS setup, and serial port connection/reconnection.
*/
int main(int argc, char **argv)
{
//ros::init(argc, argv, "um7_driver");
using_shared_memory();
// Load parameters from private node handle.
std::string port("/dev/robot/imu");
int32_t baud = 115200;
//ros::param::param<std::string>("~port", port, "/dev/ttyUSB0");
//ros::param::param<int32_t>("~baud", baud, 115200);
serial::Serial ser;
ser.setPort(port);
ser.setBaudrate(baud);
serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0);
ser.setTimeout(to);
//ros::NodeHandle n;
//std_msgs::Header header;
//ros::param::param<std::string>("~frame_id", header.frame_id, "imu_link");
// Initialize covariance. The UM7 sensor does not provide covariance values so,
// by default, this driver provides a covariance array of all zeros indicating
// "covariance unknown" as advised in sensor_msgs/Imu.h.
// This param allows the user to specify alternate covariance values if needed.
// std::string covariance;
// char cov[200];
// char *ptr1;
// ros::param::param<std::string>("~covariance", covariance, "0 0 0 0 0 0 0 0 0");
// snprintf(cov, sizeof(cov), "%s", covariance.c_str());
// char* p = strtok_r(cov, " ", &ptr1); // point to first value
// for (int iter = 0; iter < 9; iter++)
// {
// if (p) covar[iter] = atof(p); // covar[] is global var
// else covar[iter] = 0.0;
// p = strtok_r(NULL, " ", &ptr1); // point to next value (nil if none)
// }
// Real Time Loop
bool first_failure = true;
while (1)
{
try
{
ser.open();
}
catch(const serial::IOException& e)
{
std::cout<<"Unable to connect to port."<<std::endl;
}
if (ser.isOpen())
{
std::cout<<"Successfully connected to serial port."<<std::endl;
first_failure = true;
try
{
um7::Comms sensor(&ser);
configureSensor(&sensor);
um7::Registers registers;
//ros::ServiceServer srv = n.advertiseService<um7::Reset::Request, um7::Reset::Response>(
// "reset", boost::bind(handleResetService, &sensor, _1, _2));
handleResetService(&sensor);
int t=0;
int contador = 0;
float med_accel_z = 0, ac_med_accel_z = 0;
while (1)
{
//--------- calcula a média do accel em Z-----------------------------
if(contador>=40)
{
med_accel_z = ac_med_accel_z/40; // calcula a média do accel em Z
contador = 0;
ac_med_accel_z = 0;
}
ac_med_accel_z = ac_med_accel_z + IMU_ACCEL_Z;
contador++;
//--------------------------------------------------------------------
if(med_accel_z>0.70) // Identifica se o robô esta caido ou em pé
IMU_STATE = 0; // Robo caido
else
IMU_STATE = 1; // Robo em pé
if(t>20)
{
std::cout<<"Robo caido = "<<std::fixed<<IMU_STATE<<std::endl;
std::cout<<"med_acc_z = "<<std::fixed<<med_accel_z<<std::endl;
std::cout<<"giros_x = "<<std::fixed<<IMU_GYRO_X<<std::endl;
std::cout<<"giros_y = "<<std::fixed<<IMU_GYRO_Y<<std::endl;
std::cout<<"giros_z = "<<std::fixed<<IMU_GYRO_Z<<std::endl;
std::cout<<"accel_x = "<<std::fixed<<IMU_ACCEL_X<<std::endl;
std::cout<<"accel_y = "<<std::fixed<<IMU_ACCEL_Y<<std::endl;
std::cout<<"accel_z = "<<std::fixed<<IMU_ACCEL_Z<<std::endl;
std::cout<<"magne_x = "<<std::fixed<<IMU_COMPASS_X<<std::endl;
std::cout<<"magne_y = "<<std::fixed<<IMU_COMPASS_Y<<std::endl;
std::cout<<"magne_z = "<<std::fixed<<IMU_COMPASS_Z<<std::endl;
std::cout<<"Quat_x = "<<std::fixed<<IMU_QUAT_X<<std::endl;
std::cout<<"Quat_y = "<<std::fixed<<IMU_QUAT_Y<<std::endl;
std::cout<<"Quat_z = "<<std::fixed<<IMU_QUAT_Z<<std::endl;
std::cout<<"Euler_x = "<<std::fixed<<IMU_EULER_X<<std::endl;
std::cout<<"Euler_y = "<<std::fixed<<IMU_EULER_Y<<std::endl;
std::cout<<"Euler_z = "<<std::fixed<<IMU_EULER_Z<<std::endl<<std::endl;
t=0;
}
t++;
// triggered by arrival of last message packet
if (sensor.receive(®isters) == TRIGGER_PACKET)
{
//header.stamp = ros::Time::now();
publishMsgs(registers);
//ros::spinOnce();
}
}
}
catch(const std::exception& e)
{
if (ser.isOpen()) ser.close();
//ROS_ERROR_STREAM(e.what());
//ROS_INFO("Attempting reconnection after error.");
std::cout<<"Attempting reconnection after error."<<std::endl;
//ros::Duration(1.0).sleep();
}
}
else
{
//ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device "
// << port << ". Trying again every 1 second.");
std::cout<< "Could not connect to serial device "
<< port << ". Trying again every 1 second."<< std::endl;
first_failure = false;
//ros::Duration(1.0).sleep();
}
}
}
/**
* start is overridden to allow retrieving initial calibration property before
* and to set the required value type flags
*/
void CMagnetometerSensorSym::start()
{
if(sensor())
{
// Initialize the values
iReading.setX(0);
iReading.setY(0);
iReading.setZ(0);
// Set the required type of values
QVariant v = sensor()->property("returnGeoValues");
iReturnGeoValues = (v.isValid() && v.toBool()); // if the property isn't set it's false
}
TInt err;
// get current property value for calibration and set it to reading
TSensrvProperty calibration;
TRAP(err, iBackendData.iSensorChannel->GetPropertyL(KSensrvPropCalibrationLevel, ESensrvSingleProperty, calibration));
// If error in getting the calibration level, continue to start the sensor
// as it is not a fatal error
if ( err == KErrNone )
{
TInt calibrationVal;
calibration.GetValue(calibrationVal);
iCalibrationLevel = calibrationVal * (1.0/3.0);
}
// Call backend start
CSensorBackendSym::start();
TSensrvProperty dataFormatProperty;
TRAP(err, iBackendData.iSensorChannel->GetPropertyL(KSensrvPropIdChannelDataFormat, ESensrvSingleProperty, dataFormatProperty));
if(err == KErrNone)
{
TInt dataFormat;
dataFormatProperty.GetValue(dataFormat);
if(dataFormat == ESensrvChannelDataFormatScaled)
{
TSensrvProperty scaleRangeProperty;
TRAP(err, iBackendData.iSensorChannel->GetPropertyL(KSensrvPropIdScaledRange, KSensrvItemIndexNone, scaleRangeProperty));
if(err == KErrNone)
{
if(scaleRangeProperty.GetArrayIndex() == ESensrvSingleProperty)
{
if(scaleRangeProperty.PropertyType() == ESensrvIntProperty)
{
scaleRangeProperty.GetMaxValue(iScaleRange);
}
else if(scaleRangeProperty.PropertyType() == ESensrvRealProperty)
{
TReal realScale;
scaleRangeProperty.GetMaxValue(realScale);
iScaleRange = realScale;
}
}
else if(scaleRangeProperty.GetArrayIndex() == ESensrvArrayPropertyInfo)
{
TInt index;
if(scaleRangeProperty.PropertyType() == ESensrvIntProperty)
{
scaleRangeProperty.GetValue(index);
}
else if(scaleRangeProperty.PropertyType() == ESensrvRealProperty)
{
TReal realIndex;
scaleRangeProperty.GetValue(realIndex);
index = realIndex;
}
TRAP(err, iBackendData.iSensorChannel->GetPropertyL(KSensrvPropIdScaledRange, KSensrvItemIndexNone, index, scaleRangeProperty));
if(err == KErrNone)
{
if(scaleRangeProperty.PropertyType() == ESensrvIntProperty)
{
scaleRangeProperty.GetMaxValue(iScaleRange);
}
else if(scaleRangeProperty.PropertyType() == ESensrvRealProperty)
{
TReal realScaleRange;
scaleRangeProperty.GetMaxValue(realScaleRange);
iScaleRange = realScaleRange;
}
}
}
}
}
}
}
/*
* RecvData is used to retrieve the sensor reading from sensor server
* It is implemented here to handle rotation sensor specific
* reading data and provides conversion and utility code
*/
void CRotationSensorSym::RecvData(CSensrvChannel &aChannel)
{
TPckg<TSensrvRotationData> rotationpkg( iData );
TInt ret = aChannel.GetData( rotationpkg );
if(KErrNone != ret)
{
// If there is no reading available, return without setting
return;
}
// Get a lock on the reading data
iBackendData.iReadingLock.Wait();
// To Do verify with ds and ramsay
// For x axis symbian provides reading from 0 to 359 range
// This logic maps value to Qt range -90 to 90
if(iData.iDeviceRotationAboutXAxis >= 0 && iData.iDeviceRotationAboutXAxis <= 180)
{
iReading.setX(90 - iData.iDeviceRotationAboutXAxis);
}
else if(iData.iDeviceRotationAboutXAxis > 180 && iData.iDeviceRotationAboutXAxis <= 270)
{
iReading.setX(iData.iDeviceRotationAboutXAxis - 270);
}
else if(iData.iDeviceRotationAboutXAxis > 270 && iData.iDeviceRotationAboutXAxis < 360)
{
iReading.setX(iData.iDeviceRotationAboutXAxis - 270);
}
// For y axis symbian provides reading from 0 to 359 range
// This logic maps value to Qt range -180 to 180
if(iData.iDeviceRotationAboutYAxis >= 0 && iData.iDeviceRotationAboutYAxis <= 180)
{
iReading.setY(iData.iDeviceRotationAboutYAxis);
}
else if(iData.iDeviceRotationAboutYAxis > 180 && iData.iDeviceRotationAboutYAxis < 360)
{
iReading.setY(iData.iDeviceRotationAboutYAxis - 360);
}
if(iData.iDeviceRotationAboutZAxis == TSensrvRotationData::KSensrvRotationUndefined)
{
sensor()->setProperty("hasZ", QVariant(FALSE));
}
else
{
sensor()->setProperty("hasZ", QVariant(TRUE));
// For z axis symbian provides reading from 0 to 359 range
// This logic maps value to Qt range -180 to 180
if(iData.iDeviceRotationAboutZAxis >= 0 && iData.iDeviceRotationAboutZAxis <= 180)
{
iReading.setZ(iData.iDeviceRotationAboutZAxis);
}
else if(iData.iDeviceRotationAboutZAxis > 180 && iData.iDeviceRotationAboutZAxis < 360)
{
iReading.setZ(iData.iDeviceRotationAboutZAxis - 360);
}
}
// Set the timestamp
iReading.setTimestamp(iData.iTimeStamp.Int64());
// Release the lock
iBackendData.iReadingLock.Signal();
}
bool QTQmlSensor::isBusy() const
{
return sensor()->isBusy();
}
static void makemoves(void) {
int i, hitme;
char ch;
while (TRUE) { /* command loop */
hitme = FALSE;
justin = 0;
Time = 0.0;
i = -1;
while (TRUE) { /* get a command */
chew();
skip(1);
proutn("COMMAND> ");
// Use of scan() here (after chew) will get a new line of input
// and will return IHEOL iff new line of input contains nothing
// or a numeric input is detected but conversion fails.
if (scan() == IHEOL) continue;
for (i=0; i < 26; i++)
if (isit(commands[i]))
break;
if (i < 26) break;
for (; i < NUMCOMMANDS; i++)
if (strcmp(commands[i], citem) == 0) break;
if (i < NUMCOMMANDS) break;
// we get here iff the first parsed input from the line does not
// match one of the commands. In this case, the rest of the line
// is discarded, the below message is printed, and we go back to
// get a new command.
if (skill <= 2) {
prout("UNRECOGNIZED COMMAND. LEGAL COMMANDS ARE:");
listCommands(TRUE);
}
else prout("UNRECOGNIZED COMMAND.");
} // end get command loop
// we get here iff the first parsed input from the line matches one
// of the commands (i.e., command i). We use i to dispatch the
// handling of the command. The line may still contain additional
// inputs (i.e., parameters of the command) that is to be parsed by
// the dispatched command handler. If the line does not contain
// all the necessary parameters, the dispatched command handler is
// responsible to get additional input(s) interactively using scan().
// The dispatched command handler is also responsible to handle any
// input errors.
switch (i) { /* command switch */
case 0: // srscan
srscan(1);
break;
case 1: // lrscan
lrscan();
break;
case 2: // phasers
phasers();
if (ididit) hitme = TRUE;
break;
case 3: // photons
photon();
if (ididit) hitme = TRUE;
break;
case 4: // move
warp(1);
break;
case 5: // shields
sheild(1);
if (ididit) {
attack(2);
shldchg = 0;
}
break;
case 6: // dock
dock();
break;
case 7: // damages
dreprt();
break;
case 8: // chart
chart(0);
break;
case 9: // impulse
impuls();
break;
case 10: // rest
waiting();
if (ididit) hitme = TRUE;
break;
case 11: // warp
setwrp();
break;
case 12: // status
srscan(3);
break;
case 13: // sensors
sensor();
break;
case 14: // orbit
orbit();
if (ididit) hitme = TRUE;
break;
case 15: // transport "beam"
beam();
break;
case 16: // mine
mine();
if (ididit) hitme = TRUE;
break;
case 17: // crystals
usecrystals();
break;
case 18: // shuttle
shuttle();
if (ididit) hitme = TRUE;
break;
case 19: // Planet list
preport();
break;
case 20: // Status information
srscan(2);
break;
case 21: // Game Report
report(0);
break;
case 22: // use COMPUTER!
eta();
break;
case 23:
listCommands(TRUE);
break;
case 24: // Emergency exit
clearscreen(); // Hide screen
freeze(TRUE); // forced save
exit(1); // And quick exit
break;
case 25:
probe(); // Launch probe
break;
case 26: // Abandon Ship
abandn();
break;
case 27: // Self Destruct
dstrct();
break;
case 28: // Save Game
freeze(FALSE);
if (skill > 3)
prout("WARNING--Frozen games produce no plaques!");
break;
case 29: // Try a desparation measure
deathray();
if (ididit) hitme = TRUE;
break;
case 30: // What do we want for debug???
#ifdef DEBUG
debugme();
#endif
break;
case 31: // Call for help
help();
break;
case 32:
alldone = 1; // quit the game
#ifdef DEBUG
if (idebug) score();
#endif
break;
case 33:
helpme(); // get help
break;
} // end command switch
for (;;) {
if (alldone) break; // Game has ended
#ifdef DEBUG
if (idebug) prout("2500");
#endif
if (Time != 0.0) {
events();
if (alldone) break; // Events did us in
}
if (d.galaxy[quadx][quady] == 1000) { // Galaxy went Nova!
atover(0);
continue;
}
if (nenhere == 0) movetho();
if (hitme && justin==0) {
attack(2);
if (alldone) break;
if (d.galaxy[quadx][quady] == 1000) { // went NOVA!
atover(0);
hitme = TRUE;
continue;
}
}
break;
} // end event loop
if (alldone) break;
} // end command loop
}
void QTQmlSensor::setAlwaysOn(bool alwaysOn)
{
sensor()->setAlwaysOn(alwaysOn);
}
int main(int argc, char* argv[]){
std::string pose_offset_filename = "./poseoffset";
unsigned cv_width = 128;
unsigned cv_height = 128;
unsigned cv_depth = 128;
float cv_min_d = 0.5;
float cv_max_d = 3.0;
bool undistort = false;
unsigned idwneighbours = 10;
bool using_nni = false;
CMDParser p("basefilename samplesfilename checkerboardviewinitfilename");
p.addOpt("p",1,"poseoffetfilename", "specify the filename of the poseoffset on disk, default: " + pose_offset_filename);
p.addOpt("s",3,"size", "use this calibration volume size (width x height x depth), default: 128 128 256");
p.addOpt("d",2,"depthrange", "use this depth range: 0.5 4.5");
p.addOpt("u", -1, "undistort", "enable undistortion of images before chessboardsampling, default: false");
p.addOpt("n",1,"numneighbours", "the number of neighbours that should be used for IDW inverse distance weighting, default: 10");
p.addOpt("i",-1,"nni", "do use natural neighbor interpolation if possible, default: false");
p.init(argc,argv);
if(p.getArgs().size() != 3)
p.showHelp();
if(p.isOptSet("p")){
pose_offset_filename = p.getOptsString("p")[0];
std::cout << "setting poseoffetfilename to " << pose_offset_filename << std::endl;
}
if(p.isOptSet("s")){
cv_width = p.getOptsInt("s")[0];
cv_height = p.getOptsInt("s")[1];
cv_depth = p.getOptsInt("s")[2];
}
if(p.isOptSet("d")){
cv_min_d = p.getOptsInt("d")[0];
cv_max_d = p.getOptsInt("d")[1];
}
if(p.isOptSet("u")){
undistort = true;
}
if(p.isOptSet("n")){
idwneighbours = p.getOptsInt("n")[0];
std::cout << "setting to numneighbours " << idwneighbours << std::endl;
}
if(p.isOptSet("i")){
using_nni = true;
}
std::string basefilename = p.getArgs()[0];
std::string filename_xyz(basefilename + "_xyz");
std::string filename_uv(basefilename + "_uv");
const std::string filename_yml(basefilename + "_yml");
std::string filename_samples(p.getArgs()[1]);
CalibVolume cv(cv_width, cv_height, cv_depth, cv_min_d, cv_max_d);
RGBDConfig cfg;
cfg.read(filename_yml.c_str());
RGBDSensor sensor(cfg);
Checkerboard cb;
cb.load_pose_offset(pose_offset_filename.c_str());
ChessboardSampling cbs(p.getArgs()[2].c_str(), cfg, undistort);
cbs.init();
glm::mat4 eye_d_to_world = sensor.guess_eye_d_to_world_static(cbs, cb);
std::cerr << "extrinsic of sensor is: " << eye_d_to_world << std::endl;
std::cerr << "PLEASE note, the extrinsic guess can be improved by averaging" << std::endl;
for(unsigned z = 0; z < cv.depth; ++z){
for(unsigned y = 0; y < cv.height; ++y){
for(unsigned x = 0; x < cv.width; ++x){
const unsigned cv_index = (z * cv.width * cv.height) + (y * cv.width) + x;
const float depth = (z + 0.5) * (cv.max_d - cv.min_d)/cv.depth + cv.min_d;
const float xd = (x + 0.5) * sensor.config.size_d.x * 1.0/cv.width;
const float yd = (y + 0.5) * sensor.config.size_d.y * 1.0/cv.height;
glm::vec3 pos3D_local = sensor.calc_pos_d(xd, yd, depth);
glm::vec2 pos2D_rgb = sensor.calc_pos_rgb(pos3D_local);
pos2D_rgb.x /= sensor.config.size_rgb.x;
pos2D_rgb.y /= sensor.config.size_rgb.y;
glm::vec4 pos3D_world = eye_d_to_world * glm::vec4(pos3D_local.x, pos3D_local.y, pos3D_local.z, 1.0);
xyz pos3D;
pos3D.x = pos3D_world.x;
pos3D.y = pos3D_world.y;
pos3D.z = pos3D_world.z;
cv.cv_xyz[cv_index] = pos3D;
uv posUV;
posUV.u = pos2D_rgb.x;
posUV.v = pos2D_rgb.y;
cv.cv_uv[cv_index] = posUV;
}
}
}
// load samples from filename
std::vector<samplePoint> sps;
std::ifstream iff(filename_samples.c_str(), std::ifstream::binary);
const unsigned num_samples_in_file = calcNumFrames(iff,
sizeof(float) +
sizeof(uv) +
sizeof(uv) +
sizeof(xyz) +
sizeof(uv) +
sizeof(glm::vec3) +
sizeof(float));
for(unsigned i = 0; i < num_samples_in_file; ++i){
samplePoint s;
iff.read((char*) &s.depth, sizeof(float));
iff.read((char*) &s.tex_color, sizeof(uv));
iff.read((char*) &s.tex_depth, sizeof(uv));
iff.read((char*) &s.pos_offset, sizeof(xyz));
iff.read((char*) &s.tex_offset, sizeof(uv));
iff.read((char*) glm::value_ptr(s.pos_real), sizeof(glm::vec3));
iff.read((char*) &s.quality, sizeof(float));
sps.push_back(s);
}
iff.close();
// reapply correction offsets
for(unsigned i = 0; i < sps.size(); ++i){
const unsigned cv_width = cv.width;
const unsigned cv_height = cv.height;
const unsigned cv_depth = cv.depth;
const float x = cv_width * ( sps[i].tex_depth.u) / cfg.size_d.x;
const float y = cv_height * ( sps[i].tex_depth.v)/ cfg.size_d.y;
const float z = cv_depth * ( sps[i].depth - cv.min_d)/(cv.max_d - cv.min_d);
xyz pos = getTrilinear(cv.cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
uv tex = getTrilinear(cv.cv_uv, cv_width, cv_height, cv_depth, x , y , z );
sps[i].pos_offset.x = sps[i].pos_real[0] - pos.x;
sps[i].pos_offset.y = sps[i].pos_real[1] - pos.y;
sps[i].pos_offset.z = sps[i].pos_real[2] - pos.z;
sps[i].tex_offset.u = sps[i].tex_color.u/cfg.size_rgb.x - tex.u;
sps[i].tex_offset.v = sps[i].tex_color.v/cfg.size_rgb.y - tex.v;
sps[i].quality = 1.0f;
}
Calibrator c;
c.using_nni = using_nni;
c.applySamples(&cv, sps, cfg, idwneighbours, basefilename.c_str());
cv.save(filename_xyz.c_str(), filename_uv.c_str());
return 0;
}
void QTQmlSensor::setSkipDuplicates(bool skipDuplicates)
{
sensor()->setSkipDuplicates(skipDuplicates);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "AI");
ros::NodeHandle ros_node;
ros::Rate loop_rate(20);
GameControl gameControl;
Sensors sensor(gameControl);
BrianParser parser(gameControl, sensor);
clock_t startGame = clock();
ros::Publisher motion = ros_node.advertise<SpyKee::Motion>("spykee_motion", 1000);
ros::Subscriber sonar_sub = ros_node.subscribe("sonar_data", 1000, &Sensors::sonarCallBack, &sensor);
ros::Subscriber rfid_sub = ros_node.subscribe("rfid_data", 1000, &Sensors::rfidCallBack, &sensor);
ros::Subscriber bt_sub = ros_node.subscribe("bt_data", 1000, &Sensors::btCallBack, &sensor);
ros::Subscriber ir_sub = ros_node.subscribe("ir_data", 1000, &Sensors::irCallBack, &sensor);
ros::Subscriber video_sub = ros_node.subscribe("vision_results", 1000, &Sensors::videoCallBack, &sensor);
/*
ros::ServiceClient redLedClient = ros_node.serviceClient<Echoes::FixedLed>("red_led");
ros::ServiceClient greenLedClient = ros_node.serviceClient<Echoes::BlinkingLed>("green_led");
ros::ServiceClient redResetClient = ros_node.serviceClient<Echoes::ResetLed>("reset_led");
*/
while(ros::ok())
{
parser.go();
SpyKee::Motion msg;
msg.tanSpeed = parser.getTanSpeed();
msg.rotSpeed = parser.getRotSpeed();
//msg.tanSpeed = 0;
//msg.rotSpeed = 0;
/*
if (gameControl.isSuperMode())
{
Echoes::FixedLed service;
service.request.numOn = 4;
redLedClient.call(service);
}
else
{
Echoes::ResetLed service;
redResetClient.call(service);
}
*/
if (sensor.getContact())
{
if (gameControl.isSuperMode())
cout << "The winner is PacBot" << endl;
else
cout << "The winner is GhostBot" << endl;
exit(EXIT_SUCCESS);
}
if (sensor.finishedPacDots())
{
cout << "Finished Pac Dots.\nThe winner is PacBot" << endl;
exit(EXIT_SUCCESS);
}
if (100 * (clock() - startGame) / (double) CLOCKS_PER_SEC > 15)
motion.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
static void makemoves(void) {
int i, hitme;
char ch;
while (TRUE) { /* command loop */
hitme = FALSE;
justin = 0;
Time = 0.0;
i = -1;
while (TRUE) { /* get a command */
chew();
skip(1);
proutn("COMMAND> ");
if (scan() == IHEOL) continue;
for (i=0; i < 29; i++) // Abbreviations allowed for the first 29 commands, only.
if (isit(commands[i]))
break;
if (i < 29) break;
for (; i < NUMCOMMANDS; i++)
if (strcmp(commands[i], citem) == 0) break;
if (i < NUMCOMMANDS
#ifndef CLOAKING
&& i != 26 // ignore the CLOAK command
#endif
#ifndef CAPTURE
&& i != 27 // ignore the CAPTURE command
#endif
#ifndef SCORE
&& i != 28 // ignore the SCORE command
#endif
#ifndef DEBUG
&& i != 33 // ignore the DEBUG command
#endif
) break;
if (skill <= SFAIR) {
prout("UNRECOGNIZED COMMAND. LEGAL COMMANDS ARE:");
listCommands(TRUE);
}
else prout("UNRECOGNIZED COMMAND.");
}
switch (i) { /* command switch */
case 0: // srscan
srscan(1);
break;
case 1: // lrscan
lrscan();
break;
case 2: // phasers
phasers();
if (ididit) {
#ifdef CLOAKING
if (irhere && d.date >= ALGERON && !isviolreported && iscloaked) {
prout("The Romulan ship discovers you are breaking the Treaty of Algeron!");
ncviol++;
isviolreported = TRUE;
}
#endif
hitme = TRUE;
}
break;
case 3: // photons
photon();
if (ididit) {
#ifdef CLOAKING
if (irhere && d.date >= ALGERON && !isviolreported && iscloaked) {
prout("The Romulan ship discovers you are breaking the Treaty of Algeron!");
ncviol++;
isviolreported = TRUE;
}
#endif
hitme = TRUE;
}
break;
case 4: // move
warp(1);
break;
case 5: // shields
sheild(1);
if (ididit) {
attack(2);
shldchg = 0;
}
break;
case 6: // dock
dock();
break;
case 7: // damages
dreprt();
break;
case 8: // chart
chart(0);
break;
case 9: // impulse
impuls();
break;
case 10: // rest
waiting();
if (ididit) hitme = TRUE;
break;
case 11: // warp
setwrp();
break;
case 12: // status
srscan(3);
break;
case 13: // sensors
sensor();
break;
case 14: // orbit
orbit();
if (ididit) hitme = TRUE;
break;
case 15: // transport "beam"
beam();
break;
case 16: // mine
mine();
if (ididit) hitme = TRUE;
break;
case 17: // crystals
usecrystals();
break;
case 18: // shuttle
shuttle();
if (ididit) hitme = TRUE;
break;
case 19: // Planet list
preport();
break;
case 20: // Status information
srscan(2);
break;
case 21: // Game Report
report(0);
break;
case 22: // use COMPUTER!
eta();
break;
case 23:
listCommands(TRUE);
break;
case 24: // Emergency exit
clearscreen(); // Hide screen
freeze(TRUE); // forced save
exit(1); // And quick exit
break;
case 25:
probe(); // Launch probe
break;
#ifdef CLOAKING
case 26:
cloak(); // turn on/off cloaking
if (iscloaking) {
attack(2); // We will be seen while we cloak
iscloaking = FALSE;
iscloaked = TRUE;
}
break;
#endif
#ifdef CAPTURE
case 27:
capture(); // Attempt to get Klingon ship to surrender
if (ididit) hitme = TRUE;
break;
#endif
#ifdef SCORE
case 28:
score(1); // get the score
break;
#endif
case 29: // Abandon Ship
abandn();
break;
case 30: // Self Destruct
dstrct();
break;
case 31: // Save Game
freeze(FALSE);
if (skill > SGOOD)
prout("WARNING--Frozen games produce no plaques!");
break;
case 32: // Try a desparation measure
deathray();
if (ididit) hitme = TRUE;
break;
#ifdef DEBUG
case 33: // What do we want for debug???
debugme();
break;
#endif
case 34: // Call for help
help();
break;
case 35:
alldone = 1; // quit the game
#ifdef DEBUG
if (idebug) score(0);
#endif
break;
case 36:
helpme(); // get help
break;
}
for (;;) {
if (alldone) break; // Game has ended
#ifdef DEBUG
if (idebug) prout("2500");
#endif
if (Time != 0.0) {
events();
if (alldone) break; // Events did us in
}
if (d.galaxy[quadx][quady] == 1000) { // Galaxy went Nova!
atover(0);
continue;
}
if (nenhere == 0) movetho();
if (hitme && justin==0) {
attack(2);
if (alldone) break;
if (d.galaxy[quadx][quady] == 1000) { // went NOVA!
atover(0);
hitme = TRUE;
continue;
}
}
break;
}
if (alldone) break;
}
}
void EthernetSensorBase::addTo(EthernetSensorContainer& map) {
map.insert(getMessageId(), sensor());
}
bool parse_sensors_internal(Vector<jsmntok_t>& tokens_vector, const char* json_buffer, Vector<SensorPtr>& sensors)
{
int sensors_array_token_index, current_token_index, sensor_index;
jsmntok_t* tokens = &tokens_vector[0];
// Find sensors array token:
sensors_array_token_index = find_json_array_token(tokens_vector, json_buffer, SENSORS_TAG);
if (sensors_array_token_index < 0) {
return false;
}
current_token_index = sensors_array_token_index + 1;
// iterate of all tokens, try to build sensors
for (sensor_index = 0 ; sensor_index < tokens[sensors_array_token_index].size; sensor_index++) {
int id = -1, port_index = -1, value;
double sensor_value = 0;
SensorType mode = MOCK;
const unsigned int next_sensor_token_index = current_token_index + tokens[current_token_index].size + 1;
// We're expecting something like - {"id":4,"port_index":6, "type":"water_meter", "value":8.0}
if (tokens[current_token_index].type != JSMN_OBJECT || tokens[current_token_index].size < 4) {
current_token_index = next_sensor_token_index;
continue;
}
current_token_index++;
// First token is the key, the second is the value
while (current_token_index < next_sensor_token_index) {
const unsigned int next_object_token_index = current_token_index + tokens[current_token_index].size + 1;
if (tokens[current_token_index].type != JSMN_STRING) { // Must be an error...
current_token_index = next_object_token_index;
continue;
}
if (tokens[current_token_index + 1].type == JSMN_STRING) { // probably type
if (TOKEN_STRING(json_buffer, tokens[current_token_index], SENSOR_TYPE_TAG)) {
if (TOKEN_STRING(json_buffer, tokens[current_token_index + 1], SENSOR_TYPE_WATER)) {
mode = WATER_READER;
} else if (TOKEN_STRING(json_buffer, tokens[current_token_index + 1], SENSOR_TYPE_BATTERY)) {
mode = BATTERY;
}
// TODO - add other sensor types.
// TODO - what to do in case of an error? - currently ignore.
} // else - ignore this key.
}
else if (tokens[current_token_index + 1].type == JSMN_PRIMITIVE) {
// Read the value
if (TOKEN_STRING(json_buffer, tokens[current_token_index], VALUE_TAG)) { // Value tag
if (!token_to_double(json_buffer, &tokens[current_token_index + 1], sensor_value)) {
current_token_index = next_object_token_index;
continue;
}
}
if (!token_to_int(json_buffer, &tokens[current_token_index + 1], value)) {
current_token_index = next_object_token_index;
continue;
}
if (TOKEN_STRING(json_buffer, tokens[current_token_index], ID_TAG)) { // Id tag
id = value;
} else if (TOKEN_STRING(json_buffer, tokens[current_token_index], PORT_INDEX_TAG)) { // Port index tag
port_index = value;
} // else - ignore this key.
}
current_token_index += 2;
}
if (id >= 0 && port_index >= 0) { // Add sensor
SensorPtr sensor(SensorFactory::CreateSensor(mode, id, port_index, sensor_value));
sensors.Add(sensor);
}
}
return true;
}
void controller_inputs(MBSdataStruct *MBSdata)
{
UserIOStruct *uvs;
ControllerStruct *cvs;
ControllerInputsStruct *ivs;
MBSsensorStruct S_MidWaist;
int i;
double R_11, R_21;
int robotran_id;
int robotran_id_table[] = {
WAIST_YAW, // 1. TORSO_YAW
WAIST_SAG, // 2. TORSO_PITCH
WAIST_LAT, // 3. TORSO_ROLL
R_HIP_SAG, // 4. RIGHT_HIP_PITCH
L_HIP_SAG, // 5. LEFT_HIP_PITCH
R_HIP_LAT, // 6. RIGHT_HIP_ROLL
R_HIP_YAW, // 7. RIGHT_HIP_YAW
R_KNEE_SAG, // 8. RIGHT_KNEE_PITCH
R_ANK_SAG, // 9. RIGHT_FOOT_PITCH
R_ANK_LAT, // 10. RIGHT_FOOT_ROLL
L_HIP_LAT, // 11. LEFT_HIP_ROLL
L_HIP_YAW, // 12. LEFT_HIP_YAW
L_KNEE_SAG, // 13. LEFT_KNEE_PITCH
L_ANK_SAG, // 14. LEFT_FOOT_PITCH
L_ANK_LAT, // 15. LEFT_FOOT_ROLL
R_SH_SAG, // 16. RIGHT_SHOULDER_PITCH
R_SH_LAT, // 17. RIGHT_SHOULDER_ROLL
R_SH_YAW, // 18. RIGHT_SHOULDER_YAW
R_ELB, // 19. RIGHT_ELBOW_PITCH
L_SH_SAG, // 20. LEFT_SHOULDER_PITCH
L_SH_LAT, // 21. LEFT_SHOULDER_ROLL
L_SH_YAW, // 22. LEFT_SHOULDER_YAW
L_ELB, // 23. LEFT_ELBOW_PITCH
};
uvs = MBSdata->user_IO;
cvs = uvs->cvs;
ivs = cvs->Inputs;
// -- Time -- //
ivs->t = MBSdata->tsim; // time [s]
// ---- Forces under the feet ---- //
// Vertical forces [N]
for (i=0; i<3; i++)
{
ivs->F_Rfoot[i] = uvs->GRF_r[i+1];
ivs->F_Lfoot[i] = uvs->GRF_l[i+1];
ivs->T_Rfoot[i] = uvs->GRM_r[i+1];
ivs->T_Lfoot[i] = uvs->GRM_l[i+1];
#ifdef COMP_COMAN
ivs->F_Rfoot[i] += uvs->GRF_r_dist[i+1];
ivs->F_Lfoot[i] += uvs->GRF_l_dist[i+1];
ivs->T_Rfoot[i] += uvs->GRM_r_dist[i+1];
ivs->T_Lfoot[i] += uvs->GRM_l_dist[i+1];
#endif
}
// -- Joint positions, velocities and torques -- //
for(i=0; i<COMAN_NB_JOINT_ACTUATED; i++)
{
robotran_id = robotran_id_table[i];
ivs->q[i] = MBSdata->q[robotran_id]; // position [rad]
ivs->qd[i] = MBSdata->qd[robotran_id]; // velocity [rad/s]
ivs->Qq[i] = MBSdata->Qq[robotran_id]; // torque [Nm]
}
// -- IMU -- //
// allocation
allocate_sensor(&S_MidWaist,COMAN_NB_JOINT_TOTAL);
init_sensor(&S_MidWaist,COMAN_NB_JOINT_TOTAL);
sensor(&S_MidWaist, MBSdata, S_MIDWAIST); // IMU located in the MidWaist body
ivs->IMU_Orientation[0] = S_MidWaist.R[1][1];
ivs->IMU_Orientation[1] = S_MidWaist.R[1][2];
ivs->IMU_Orientation[2] = S_MidWaist.R[1][3];
ivs->IMU_Orientation[3] = S_MidWaist.R[2][1];
ivs->IMU_Orientation[4] = S_MidWaist.R[2][2];
ivs->IMU_Orientation[5] = S_MidWaist.R[2][3];
ivs->IMU_Orientation[6] = S_MidWaist.R[3][1];
ivs->IMU_Orientation[7] = S_MidWaist.R[3][2];
ivs->IMU_Orientation[8] = S_MidWaist.R[3][3];
ivs->IMU_Angular_Rate[0] = S_MidWaist.OM[1];
ivs->IMU_Angular_Rate[1] = S_MidWaist.OM[2];
ivs->IMU_Angular_Rate[2] = S_MidWaist.OM[3];
free_sensor(&S_MidWaist);
// -- IMU not available on the real CoMan -- //
// rotation matrix
R_11 = ivs->IMU_Orientation[0];
R_21 = ivs->IMU_Orientation[3];
// absolute orientation [rad]
uvs->real_theta_3_waist = atan2(-R_21,R_11);
// derivative of absolute orientation [rad/s]
uvs->real_omega_3_waist = ivs->IMU_Angular_Rate[2];
}
