vector<QueryPoint> DictionaryTrajectory::mapFiles(vector<string> queryFiles, vector<string> trajFiles, string prefix1, string prefix2) {
vector<QueryPoint> ret;
int prefix1Size = prefix1.size();
int prefix2Size = prefix2.size();
int querySize = queryFiles.size();
int trajSize = trajFiles.size();
for(int i = 0; i < querySize; ++i) {
string currentQueryFile = string(queryFiles.at(i));
string queryAppendix = currentQueryFile.substr(prefix1Size, currentQueryFile.size() - 1);
for(int j = 0; j < trajSize; ++j) {
string currentTrajFile = string(trajFiles.at(j));
string trajAppendix = currentTrajFile.substr(prefix2Size, currentTrajFile.size() - 1);
if(!queryAppendix.compare(trajAppendix)) {
QueryPoint toAdd(queryFiles.at(i), trajFiles.at(j), string("dmp") + trajAppendix, KUKADU_SHARED_PTR<Dmp>(new JointDmp()), vec());
ret.push_back(toAdd);
if(i == 0)
startingPos = toAdd.getDmp()->getY0();
}
}
}
return ret;
}
vector<QueryPoint> DMPGeneralizer::mapFiles(vector<string> queryFiles, vector<string> trajFiles, string prefix1, string prefix2) {
vector<QueryPoint> ret;
int prefix1Size = prefix1.size();
int prefix2Size = prefix2.size();
int querySize = queryFiles.size();
int trajSize = trajFiles.size();
for(int i = 0; i < querySize; ++i) {
string currentQueryFile = string(queryFiles.at(i));
string queryAppendix = currentQueryFile.substr(prefix1Size, currentQueryFile.size() - 1);
for(int j = 0; j < trajSize; ++j) {
string currentTrajFile = string(trajFiles.at(j));
string trajAppendix = currentTrajFile.substr(prefix2Size, currentTrajFile.size() - 1);
if(!queryAppendix.compare(trajAppendix)) {
// QueryPoint(std::string fileQueryPath, std::string fileDataPath, Dmp dmp, arma::vec queryPoint);
QueryPoint toAdd(queryFiles.at(i), trajFiles.at(j), string("dmp_") + trajAppendix + string(".txt"), KUKADU_SHARED_PTR<JointDmp>(new JointDmp()), vec());
ret.push_back(toAdd);
}
}
}
return ret;
}
//! add a node in the graph
//! @param[in] nameNode name of the node
//! @param[in] cost generic node cost
void AOgraph::addNode(string nameNode, int cost)
{
// create the node
AOnode toAdd(nameNode, cost);
// add it to the set of nodes in the graph
graph.push_back(toAdd);
}
void updateIms(Point3f& start_pt)
{
glThreads[startThread]->updateThreadPoints();
vector<Vector3d> points = glThreads[startThread]->points;
int num_pts = 300;
vector<cv::Point3f> points_to_proj;
for (int i=0; i < points.size()-1; i++)
{
Vector3d vec_between = points[i+1]-points[i];
for (int j=0; j <= num_pts; j++)
{
Vector3d nextPoint = points[i]+( ((double)j)/((double)num_pts))*vec_between;
cv::Point3f toAdd((float)nextPoint(0), (float)nextPoint(1), (float)nextPoint(2));
points_to_proj.push_back(toAdd);
}
}
Mat ims[3];
for (int i=0; i < 3; i++){
ims[i] = cv::Mat::zeros(thread_vision._frames[i].size(), CV_8UC3);
}
Point2i points2d[NUMCAMS];
for (int i=0; i < points_to_proj.size(); i++)
{
thread_vision._cams->project3dPoint(points_to_proj[i], points2d);
for (int j=0; j < NUMCAMS; j++)
{
if (thread_vision._captures[j]->inRange(points2d[j].y, points2d[j].x))
{
ims[j].at<Vec3b>(points2d[j].y, points2d[j].x)[0] = (unsigned char)255;
ims[j].at<Vec3b>(points2d[j].y, points2d[j].x)[1] = (unsigned char)255;
ims[j].at<Vec3b>(points2d[j].y, points2d[j].x)[2] = (unsigned char)255;
}
}
}
start_pt = points_to_proj[0];
/* imshow("1", ims[0]);
imshow("2", ims[1]);
imshow("3", ims[2]);
*/
char im_name[256];
for (int cam_ind = 0; cam_ind < NUMCAMS; cam_ind++)
{
sprintf(im_name, "./stereo_test/stereo_test%d-%d.tif", cam_ind+1, curr_im_ind);
imwrite(im_name, ims[cam_ind]);
}
curr_im_ind++;
}
LoggerPtr Logger::getLogger(std::string const& name) {
LoggerPtr rtnValue = NULL;
log_ctr_t::iterator iter = LogManager::instance().knownLoggers.find(name);
if (LogManager::instance().knownLoggers.end() == iter) {
log_ctr_t::value_type toAdd(name, configure(name));
LogManager::instance().knownLoggers.insert(toAdd);
rtnValue = toAdd.second.get();
} else {
rtnValue = iter->second.get();
}
return rtnValue;
}
void CCLayerAnimationController::replaceImplThreadAnimations(CCLayerAnimationController* controllerImpl) const
{
controllerImpl->m_activeAnimations.clear();
for (size_t i = 0; i < m_activeAnimations.size(); ++i) {
OwnPtr<CCActiveAnimation> toAdd(m_activeAnimations[i]->cloneForImplThread());
if (m_activeAnimations[i]->needsSynchronizedStartTime()) {
// We haven't received an animation started notification yet, so it
// is important that we add it in a 'waiting' and not 'running' state.
toAdd->setRunState(CCActiveAnimation::WaitingForTargetAvailability, 0);
toAdd->setStartTime(0);
}
controllerImpl->addAnimation(toAdd.release());
}
}
void CCLayerAnimationController::replaceImplThreadAnimations(CCLayerAnimationController* controllerImpl) const
{
controllerImpl->m_activeAnimations.clear();
for (size_t i = 0; i < m_activeAnimations.size(); ++i) {
OwnPtr<CCActiveAnimation> toAdd(m_activeAnimations[i]->cloneForImplThread());
if (m_activeAnimations[i]->needsSynchronizedStartTime()) {
// We haven't received an animation started notification for this active
// animation, so it is important that it be added in the WaitingForTargetAvailibility
// state so that we do eventually get notified.
toAdd->setRunState(CCActiveAnimation::WaitingForTargetAvailability, 0);
toAdd->setStartTime(0);
}
controllerImpl->add(toAdd.release());
}
}
bool GraphicsLayerChromium::addAnimation(const KeyframeValueList& values, const IntSize& boxSize, const Animation* animation, const String& animationName, double timeOffset)
{
primaryLayer().setAnimationDelegate(this);
int animationId = mapAnimationNameToId(animationName);
int groupId = AnimationIdVendor::getNextGroupId();
OwnPtr<WebKit::WebAnimation> toAdd(createWebAnimation(values, animation, animationId, groupId, timeOffset, boxSize));
if (toAdd.get()) {
// Remove any existing animations with the same animation id and target property.
primaryLayer().removeAnimation(animationId, toAdd->targetProperty());
return primaryLayer().addAnimation(*toAdd);
}
return false;
}
//Throw the dialog to input the expense information
void Tracker::addexpensewindow()
{
addExpenseWindow = new addExpenseDialog(); // Be sure to destroy you window somewhere
addExpenseWindow->setMaximumSize(300,250);
addExpenseWindow->setMinimumSize(300,250);
addExpenseWindow->setWindowTitle("Add Expense Item on This Day");
//******Add labels******//
for(unsigned i = 0; i < expenseLabels->size(); i++){
addExpenseWindow->categoryBox->addItem(tr((*expenseLabels)[i].c_str()));
}
//addExpenseWindow->categoryBox->addItem(tr("Add new label..."));
//***if save***//
if(addExpenseWindow->exec() == QDialog::Accepted){
//***check if new label***//
unsigned i;
for(i = 0; i < expenseLabels->size(); i++){
if(addExpenseWindow->expenseLabel.toStdString() == (*expenseLabels)[i]){
break;
}
}
if(i == expenseLabels->size()){
expenseLabels->push_back(addExpenseWindow->expenseLabel.toStdString());
}
//*************************//
date testDate(calendar->selectedDate().year(),calendar->selectedDate().month(),calendar->selectedDate().day());
item toAdd(addExpenseWindow->expensename.toStdString(), addExpenseWindow->expenseamount, testDate, addExpenseWindow->expenseLabel.toStdString());
//std::cout<<toAdd.getName().c_str()<<" "<<toAdd.getAmount()<<" "<<toAdd.getLabel().c_str()<<" "<<toAdd.getDate().month<<"/"<<toAdd.getDate().day<<"/"<<toAdd.getDate().year<<std::endl;
expenseItems->push_back(toAdd);
//std::cout<<"clicked SAVE"<<std::endl;
selectedDateChanged();
}
//***if not save***//
else{
//std::cout<<"clicked CANCLE"<<std::endl;
}
double ratioValue = report_sum_M(QDate::currentDate().month(),QDate::currentDate().year(),expenseItems)/budgetAmount;
if(ratioValue < 1){
ratio->setValue(100*ratioValue);
}
else ratio->setValue(100);
}
/*private*/
std::auto_ptr<BoundableList>
STRtree::createParentBoundablesFromVerticalSlices(std::vector<BoundableList*>* verticalSlices, int newLevel)
{
assert(!verticalSlices->empty());
std::auto_ptr<BoundableList> parentBoundables( new BoundableList() );
for (size_t i=0, vssize=verticalSlices->size(); i<vssize; ++i)
{
std::auto_ptr<BoundableList> toAdd (
createParentBoundablesFromVerticalSlice(
(*verticalSlices)[i], newLevel)
);
assert(!toAdd->empty());
parentBoundables->insert(
parentBoundables->end(),
toAdd->begin(),
toAdd->end());
}
return parentBoundables;
}
bool GraphicsLayerChromium::addAnimation(const KeyframeValueList& values, const IntSize& boxSize, const Animation* animation, const String& animationName, double timeOffset)
{
platformLayer()->setAnimationDelegate(this);
int animationId = 0;
if (m_animationIdMap.contains(animationName))
animationId = m_animationIdMap.get(animationName);
OwnPtr<WebAnimation> toAdd(createWebAnimation(values, animation, animationId, timeOffset, boxSize));
if (toAdd) {
animationId = toAdd->id();
m_animationIdMap.set(animationName, animationId);
// Remove any existing animations with the same animation id and target property.
platformLayer()->removeAnimation(animationId, toAdd->targetProperty());
return platformLayer()->addAnimation(toAdd.get());
}
return false;
}
void CCLayerAnimationController::pushNewAnimationsToImplThread(CCLayerAnimationController* controllerImpl) const
{
// Any new animations owned by the main thread's controller are cloned and adde to the impl thread's controller.
for (size_t i = 0; i < m_activeAnimations.size(); ++i) {
// If the animation is already running on the impl thread, there is no need to copy it over.
if (controllerImpl->getActiveAnimation(m_activeAnimations[i]->group(), m_activeAnimations[i]->targetProperty()))
continue;
// If the animation is not running on the impl thread, it does not necessarily mean that it needs
// to be copied over and started; it may have already finished. In this case, the impl thread animation
// will have already notified that it has started and the main thread animation will no longer need
// a synchronized start time.
if (!m_activeAnimations[i]->needsSynchronizedStartTime())
continue;
OwnPtr<CCActiveAnimation> toAdd(m_activeAnimations[i]->cloneForImplThread());
ASSERT(!toAdd->needsSynchronizedStartTime());
// The new animation should be set to run as soon as possible.
toAdd->setRunState(CCActiveAnimation::WaitingForTargetAvailability, 0);
toAdd->setStartTime(0);
controllerImpl->add(toAdd.release());
}
}
vector<QueryPoint> DictionaryTrajectory::mapFiles(vector<string> queryFiles, vector<string> trajFiles, vector<string> dmpFiles, string prefix1, string prefix2, string prefix3) {
vector<QueryPoint> ret;
int prefix1Size = prefix1.size();
int prefix2Size = prefix2.size();
int prefix3Size = prefix3.size();
int querySize = queryFiles.size();
int trajSize = trajFiles.size();
int dmpSize = dmpFiles.size();
for(int i = 0; i < querySize; ++i) {
string currentQueryFile = string(queryFiles.at(i));
string queryAppendix = currentQueryFile.substr(prefix1Size, currentQueryFile.size() - 1);
for(int j = 0; j < trajSize; ++j) {
string currentTrajFile = string(trajFiles.at(j));
string trajAppendix = currentTrajFile.substr(prefix2Size, currentTrajFile.size() - 1);
if(!queryAppendix.compare(trajAppendix)) {
for(int k = 0; k < dmpSize; ++k) {
string currentDmpFile = string(dmpFiles.at(k));
string dmpAppendix = currentDmpFile.substr(prefix3Size, currentDmpFile.size() - 1);
if(!dmpAppendix.compare(queryAppendix)) {
// load dmp from file
QueryPoint toAdd(queryFiles.at(i), trajFiles.at(j), prefix3 + trajAppendix, KUKADU_SHARED_PTR<Dmp>(new JointDmp(baseFolder + prefix3 + trajAppendix)), vec());
toAdd.setQueryPoint(readQuery(string(baseFolder) + string(toAdd.getFileQueryPath())));
ret.push_back(toAdd);
if(i == 0)
startingPos = toAdd.getDmp()->getY0();
}
}
}
}
}
return ret;
}
void XbeeCoordinator::processXbeeMessage(const std::string& sender,
const std::string& /*networkAddress*/,
const std::string& data,
const bool /*broadcast*/)
{
std::string msg = data.substr(0,2);
ROS_DEBUG_STREAM("Xbee: rf message from" << sender << ":" << data);
if(msg == "HI")
{
ROS_INFO_STREAM("New Xbee in system: " << sender);
RobotState toAdd(sender, data.substr(2));
m_robotInfos[sender] = toAdd;
//give 5 extra seconds to complete startup before the node is stale
m_robotInfos[sender].lastHeartbeat = ros::Time::now()+ros::Duration(5);
std::string sendData = "AK " + m_xbee.getAddress();
m_xbee.sendTransmitPacket(sendData, sender);
} else if(msg == "ST")
{
m_robotInfos[sender].lastHeartbeat = ros::Time::now();
} else if(msg == "AK")
{
ROS_ERROR("XbeeCoordinator: Received AK message, there may be another\
coordinator with address %s", sender.c_str());
} else if (msg == "OD")
//Throw the dialog to input the income information
void Tracker::addincomewindow()
{
addIncomeWindow = new addIncomeDialog(); // Be sure to destroy you window somewhere
addIncomeWindow->setMaximumSize(300,250);
addIncomeWindow->setMinimumSize(300,250);
addIncomeWindow->setWindowTitle("Add Income Item on This Day");
//******Add labels******//
for(unsigned i = 0; i < incomeLabels->size(); i++){
addIncomeWindow->categoryBox->addItem(tr((*incomeLabels)[i].c_str()));
}
//addIncomeWindow->categoryBox->addItem(tr("Add new label..."));
//***if save***//
if(addIncomeWindow->exec() == QDialog::Accepted){
//***check if new label***//
unsigned i;
for(i = 0; i < incomeLabels->size(); i++){
if(addIncomeWindow->incomeLabel.toStdString() == (*incomeLabels)[i]){
break;
}
}
if(i == incomeLabels->size()){
incomeLabels->push_back(addIncomeWindow->incomeLabel.toStdString());
}
//*************************//
date testDate(calendar->selectedDate().year(),calendar->selectedDate().month(),calendar->selectedDate().day());
item toAdd(addIncomeWindow->incomename.toStdString(), addIncomeWindow->incomeamount, testDate, addIncomeWindow->incomeLabel.toStdString());
//std::cout<<toAdd.getName().c_str()<<" "<<toAdd.getAmount()<<" "<<toAdd.getLabel().c_str()<<" "<<toAdd.getDate().month<<"/"<<toAdd.getDate().day<<"/"<<toAdd.getDate().year<<std::endl;
incomeItems->push_back(toAdd);
//std::cout<<"clicked SAVE"<<std::endl;
selectedDateChanged();
}
//***if not save***//
else{
//std::cout<<"clicked CANCLE"<<std::endl;
}
}
void MEmbedFileOpenHook::AddEmbeddedFile(const char* name, const char* file, long size)
{
M_PROFILE_SCOPE(MEmbedFileOpenHook::AddEmbeddedFile);
MEmbedFile toAdd(name, file, size);
m_Files.push_back(toAdd);
}
vector<vector<unsigned int> > lexicographicCombinations(unsigned int unN, unsigned int unT)
{
if (unN <= unT)
{
throw invalid_argument("First parameter must be greater than second parameter.");
}
vector<vector<unsigned int> > combinations;
vector<unsigned int> cursor;
cursor.push_back(0);
unsigned int j = 1, x;
for(; j <= unT; j++)
{
cursor.push_back(j-1);
}
// add two more sentinels, we will remove them later
cursor.push_back(unN);
cursor.push_back(0);
j = unT;
VISIT:
vector<unsigned int>::iterator first = cursor.begin()+1;
vector<unsigned int>::iterator last = cursor.begin()+unT+1;
vector<unsigned int> toAdd(first,last);
combinations.push_back(toAdd);
if ( j > 0)
{
x = j;
goto INCREASE;
}
// EASY_CASE
if (cursor[1] + 1 < cursor[2])
{
cursor[1] = cursor[1] + 1;
goto VISIT;
}
else
{
j = 2;
}
FIND:
cursor[j-1] = j-2;
x = cursor[j] + 1;
if (x == cursor[j+1])
{
j = j+1;
goto FIND;
}
if (j > unT)
{
return combinations;
}
INCREASE:
cursor[j] = x;
j = j-1;
goto VISIT;
return combinations;
}
std::vector<problem::state*> AStarSearch::search(problem::state* start)
{
std::multiset<fringeData> fringe;
std::vector<problem::state*> empty;
if(p->atGoal(start)) //you're dumb.
{
empty.push_back(start);
return empty;
}
fringe.insert(fringeData(start, 0, p->heuristic(start), empty));
int DELETEcounter=0;
while(true)
{
DELETEcounter++;
assert(!fringe.empty());
fringeData curr=*fringe.begin(); //temporarily store the element that we're exploring.
fringe.erase(fringe.begin());
if(p->atGoal(curr.position)) //we're done, hurray!
{
std::cout << "Nodes expanded: " << DELETEcounter << "\n";
curr.stepsSoFar.push_back(curr.position); //put in the most recently made move. Don't need to copy it because if I did, I'd have to delete it anyway before returning.
fringeData toDelete = *(fringe.begin());
while(!fringe.empty())
{
toDelete=*(fringe.begin());
fringe.erase(fringe.begin());
toDelete.deleteSelf();
}
return curr.stepsSoFar;
}
problem::state* successors[p->maxSuccessors];
int size=p->getSuccessors(curr.position, successors);
for(unsigned int i=0; i<size; i++) //add all the successors to the fringe.
{
fringeData toAdd(successors[i], curr.costToDate+1, p->heuristic(successors[i]), curr.copySteps());
toAdd.stepsSoFar.push_back(curr.position->create()); //put in the most recently made move. New memory every time
std::pair<std::multiset<fringeData>::iterator, std::multiset<fringeData>::iterator> it = fringe.equal_range(toAdd);
if(it.first!=it.second)
{
bool good=true;
for(std::multiset<fringeData>::iterator j=it.first; j!=it.second; j++)
{
if( ((*j).position)->equals(successors[i])) //the position I want to add to the fringe is already there. Can only happen once, fringe contains unique positions because of this code.
{
good=false;
if(curr.costToDate+1<(*j).costToDate) //the one I found currently is better than the previous best one
{
fringeData toDelete=*j;
fringe.erase(j);
toDelete.deleteSelf();
fringe.insert(toAdd);
}
else //the only case wheretoAdd is not used. Everywhere else I need to put it in the fringe, so do not delete.
{
toAdd.deleteSelf();
}
break;
}
}
if(good) //the point I'm trying to add is not already in the set
fringe.insert(toAdd);
}
else
fringe.insert(toAdd); //1 is because each exploration is 1 timestep, so constant cost per node
//std::push_heap(fringe.begin(), fringe.end());
}
curr.deleteSelf(); //don't forget to delete the one I'm pulling out of the fringe.
}
}
void polygonManager::loadMap(const map* toLoad)
{
polygon **cubes;
string name;
int *numTris;
int max;
ifstream fin (toLoad->getCubeInfo().c_str());
fin>>max;
for(int x=0;x<max;x++)
{
fin>>name;
texNames.push_back(name);
}
fin>>max;
cubes= new polygon*[max];
numTris=new int [max];
for(int x=0;x<max;x++)
{
fin>>numTris[x];
cubes[x]=new polygon[numTris[x]];
for(int y=0;y<numTris[x];y++)
{
fin>>cubes[x][y].norms[0]
>>cubes[x][y].norms[1]
>>cubes[x][y].norms[2];
for(int a=0;a<3;a++)
{
fin>>cubes[x][y].texCoord[a][0];
fin>>cubes[x][y].texCoord[a][1];
fin>>cubes[x][y].verts[a][0];
fin>>cubes[x][y].verts[a][1];
fin>>cubes[x][y].verts[a][2];
}
}
}
int deb=0;
float scale=2;
for(int x=0;x<toLoad->getMaxX();x++)
for(int y=0;y<toLoad->getMaxY();y++)
for(int z=0;z<toLoad->getMaxZ();z++)
{
int n=toLoad->getObj(x,y,z),m=toLoad->getMaterial(x,y,z);
for(int a=0;a<numTris[n];a++)
{
polygon tmp(cubes[n][a].texCoord,m,cubes[n][a].norms,cubes[n][a].verts);
Vec3f toScale(8,16,8);
Vec3f toAdd(-toScale[0]*x,toScale[1]*z,-toScale[2]*y);
Vec3f scaler (4,8,4);
tmp.verts[0]*=scaler;
tmp.verts[1]*=scaler;
tmp.verts[2]*=scaler;
tmp.verts[0]+=toAdd;
tmp.verts[1]+=toAdd;
tmp.verts[2]+=toAdd;
list.push_back(tmp);
if (tmp.verts[0][0]>xmax)xmax=tmp.verts[0][0];if (tmp.verts[0][0]<xmin)xmin=tmp.verts[0][0];
if (tmp.verts[0][1]>ymax)ymax=tmp.verts[0][1];if (tmp.verts[0][1]<ymin)ymin=tmp.verts[0][1];
if (tmp.verts[0][2]>zmax)zmax=tmp.verts[0][2];if (tmp.verts[0][2]<zmin)zmin=tmp.verts[0][2];
if (tmp.verts[1][0]>xmax)xmax=tmp.verts[1][0];if (tmp.verts[1][0]<xmin)xmin=tmp.verts[1][0];
if (tmp.verts[1][1]>ymax)ymax=tmp.verts[1][1];if (tmp.verts[1][1]<ymin)ymin=tmp.verts[1][1];
if (tmp.verts[1][2]>zmax)zmax=tmp.verts[1][2];if (tmp.verts[1][2]<zmin)zmin=tmp.verts[1][2];
if (tmp.verts[2][0]>xmax)xmax=tmp.verts[2][0];if (tmp.verts[2][0]<xmin)xmin=tmp.verts[2][0];
if (tmp.verts[2][1]>ymax)ymax=tmp.verts[2][1];if (tmp.verts[2][1]<ymin)ymin=tmp.verts[2][1];
if (tmp.verts[2][2]>zmax)zmax=tmp.verts[2][2];if (tmp.verts[2][2]<zmin)zmin=tmp.verts[2][2];
}
}
}
int Moving::addToTruck(char* n, double w, double v)
{
Item toAdd(n, w, v);
truck.push(toAdd);
}
int Moving::addToDonations(char* n, double w, double v)
{
Item toAdd(n, w, v);
donations.enqueue(toAdd);
}
bool
UsdGeomPointInstancer::DeactivateIds(VtInt64Array const &ids) const
{
std::vector<int64_t> toAdd(ids.begin(), ids.end());
return _SetOrMergeOverOp(toAdd, SdfListOpTypeAdded, GetPrim());
}
bool
UsdGeomPointInstancer::DeactivateId(int64_t id) const
{
std::vector<int64_t> toAdd(1, id);
return _SetOrMergeOverOp(toAdd, SdfListOpTypeAdded, GetPrim());
}
int Moving::addToUStore(char* n, double w, double v)
{
Item toAdd(n, w, v);
ustore.push(toAdd);
}
//New version for search from both ends
void updateIms(Point3f& start_pt, Vector3d& start_tan, Point3f& end_pt, Vector3d& end_tan)
{
#ifdef FAKEIMS
glThreads[truthThread]->updateThreadPoints();
vector<Vector3d> points = glThreads[truthThread]->points;
int num_pts = 300;
vector<cv::Point3f> points_to_proj;
/* Interpolate between points linearly for the generated image */
for (int i=0; i < points.size()-1; i++)
{
Vector3d vec_between = points[i+1]-points[i];
for (int j=0; j <= num_pts; j++)
{
Vector3d nextPoint = points[i]+( ((double)j)/((double)num_pts))*vec_between;
cv::Point3f toAdd((float)nextPoint(0), (float)nextPoint(1), (float)nextPoint(2));
points_to_proj.push_back(toAdd);
}
}
Mat ims[3];
for (int i=0; i < 3; i++){
ims[i] = cv::Mat::zeros(thread_vision._frames[i].size(), CV_8UC3);
}
Point2i points2d[NUMCAMS];
for (int i=0; i < points_to_proj.size(); i++)
{
thread_vision._cams->project3dPoint(points_to_proj[i], points2d);
for (int j=0; j < NUMCAMS; j++)
{
if (thread_vision._captures[j]->inRange(points2d[j].y, points2d[j].x))
{
ims[j].at<Vec3b>(points2d[j].y, points2d[j].x)[0] = (unsigned char)255;
ims[j].at<Vec3b>(points2d[j].y, points2d[j].x)[1] = (unsigned char)255;
ims[j].at<Vec3b>(points2d[j].y, points2d[j].x)[2] = (unsigned char)255;
}
}
}
char im_name[256];
for (int cam_ind = 0; cam_ind < NUMCAMS; cam_ind++)
{
sprintf(im_name, "./stereo_test/stereo_test%d-%d.tif", cam_ind+1, curr_im_ind);
imwrite(im_name, ims[cam_ind]);
}
curr_im_ind++;
EigenToOpencv(points.front(), start_pt);
EigenToOpencv(points.back(), end_pt);
start_tan = (points[1]-points[0]).normalized();
end_tan = (points[points.size() - 2] - points[points.size() - 1]).normalized();
#else
thread_vision._cams->setImageNumber(thread_ind+1);
start_pt = _start_pt;
end_pt = _end_pt;
start_tan = _start_tan;
end_tan = _end_tan;
#endif
}
static
bool
_SetOrMergeOverOp(std::vector<int64_t> const &items, SdfListOpType op,
UsdPrim const &prim)
{
SdfInt64ListOp proposed, current;
UsdStagePtr stage = prim.GetStage();
UsdEditTarget editTarget = stage->GetEditTarget();
SdfPrimSpecHandle primSpec =
editTarget.GetPrimSpecForScenePath(prim.GetPath());
if (primSpec){
VtValue existingOp = primSpec->GetInfo(UsdGeomTokens->inactiveIds);
if (existingOp.IsHolding<SdfInt64ListOp>()){
current = existingOp.UncheckedGet<SdfInt64ListOp>();
}
}
proposed.SetItems(items, op);
if (current.IsExplicit()){
std::vector<int64_t> explicitItems = current.GetExplicitItems();
proposed.ApplyOperations(&explicitItems);
current.SetExplicitItems(explicitItems);
}
else {
// We can't use ApplyOperations on an extant, non-explicit listOp
// because the result is always flat and explicit.
current.ComposeOperations(proposed, op);
// ComposeOperations() is too narrow in functionality - it does not
// consider that if we "remove over" an existing set of added items,
// we need to additionally ensure the removed items get removed
// from the added in current, since when applying ops, we first
// remove, then add. Bug #139215 filed to track; when it gets fixed
// we can remove this code!
if (op == SdfListOpTypeDeleted){
std::vector<int64_t> addedItems = current.GetAddedItems();
if (!addedItems.empty()){
std::set<int64_t> toRemove(items.begin(), items.end());
std::vector<int64_t> newAdded;
newAdded.reserve(addedItems.size());
for (auto elt : addedItems){
if (!toRemove.count(elt))
newAdded.push_back(elt);
}
if (newAdded.size() != addedItems.size())
current.SetAddedItems(newAdded);
}
}
else if (op == SdfListOpTypeAdded){
std::vector<int64_t> deletedItems = current.GetDeletedItems();
if (!deletedItems.empty()){
std::set<int64_t> toAdd(items.begin(), items.end());
std::vector<int64_t> newDeleted;
newDeleted.reserve(deletedItems.size());
for (auto elt : deletedItems){
if (!toAdd.count(elt))
newDeleted.push_back(elt);
}
if (newDeleted.size() != deletedItems.size())
current.SetDeletedItems(newDeleted);
}
}
}
return prim.SetMetadata(UsdGeomTokens->inactiveIds, current);
}
int Moving::addToLoading(char* n, double w, double v)
{
Item toAdd(n, w, v);
loading.enqueue(toAdd);
}
