void JMXStatusDCmd::execute(DCmdSource source, TRAPS) {
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
// Load and initialize the sun.management.Agent class
// invoke getManagementAgentStatus() method to generate the status info
// throw java.lang.NoSuchMethodError if method doesn't exist
Handle loader = Handle(THREAD, SystemDictionary::java_system_loader());
Klass* k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK);
instanceKlassHandle ik (THREAD, k);
JavaValue result(T_OBJECT);
JavaCalls::call_static(&result, ik, vmSymbols::getAgentStatus_name(), vmSymbols::void_string_signature(), CHECK);
jvalue* jv = (jvalue*) result.get_value_addr();
oop str = (oop) jv->l;
if (str != NULL) {
char* out = java_lang_String::as_utf8_string(str);
if (out) {
output()->print_cr("%s", out);
return;
}
}
output()->print_cr("Error obtaining management agent status");
}
// Write the method information portion of ClassFile structure
// JVMSpec| u2 methods_count;
// JVMSpec| method_info methods[methods_count];
void JvmtiClassFileReconstituter::write_method_infos() {
HandleMark hm(thread());
objArrayHandle methods(thread(), ikh()->methods());
int num_methods = methods->length();
write_u2(num_methods);
if (JvmtiExport::can_maintain_original_method_order()) {
int index;
int original_index;
int* method_order = NEW_RESOURCE_ARRAY(int, num_methods);
// invert the method order mapping
for (index = 0; index < num_methods; index++) {
original_index = ikh()->method_ordering()->int_at(index);
assert(original_index >= 0 && original_index < num_methods,
"invalid original method index");
method_order[original_index] = index;
}
// write in original order
for (original_index = 0; original_index < num_methods; original_index++) {
index = method_order[original_index];
methodHandle method(thread(), (methodOop)(ikh()->methods()->obj_at(index)));
write_method_info(method);
}
} else {
// method order not preserved just dump the method infos
for (int index = 0; index < num_methods; index++) {
void JMXStartRemoteDCmd::execute(TRAPS) {
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
// Load and initialize the sun.management.Agent class
// invoke startRemoteManagementAgent(string) method to start
// the remote management server.
// throw java.lang.NoSuchMethodError if the method doesn't exist
Handle loader = Handle(THREAD, SystemDictionary::java_system_loader());
klassOop k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK);
instanceKlassHandle ik (THREAD, k);
JavaValue result(T_VOID);
// Pass all command line arguments to java as key=value,...
// All checks are done on java side
int len = 0;
stringStream options;
char comma[2] = {0,0};
// Leave default values on Agent.class side and pass only
// agruments explicitly set by user. All arguments passed
// to jcmd override properties with the same name set by
// command line with -D or by managmenent.properties
// file.
#define PUT_OPTION(a) \
if ( (a).is_set() ){ \
options.print("%scom.sun.management.%s=%s", comma, (a).name(), (a).value()); \
comma[0] = ','; \
}
PUT_OPTION(_config_file);
PUT_OPTION(_jmxremote_port);
PUT_OPTION(_jmxremote_rmi_port);
PUT_OPTION(_jmxremote_ssl);
PUT_OPTION(_jmxremote_registry_ssl);
PUT_OPTION(_jmxremote_authenticate);
PUT_OPTION(_jmxremote_password_file);
PUT_OPTION(_jmxremote_access_file);
PUT_OPTION(_jmxremote_login_config);
PUT_OPTION(_jmxremote_ssl_enabled_cipher_suites);
PUT_OPTION(_jmxremote_ssl_enabled_protocols);
PUT_OPTION(_jmxremote_ssl_need_client_auth);
PUT_OPTION(_jmxremote_ssl_config_file);
#undef PUT_OPTION
Handle str = java_lang_String::create_from_str(options.as_string(), CHECK);
JavaCalls::call_static(&result, ik, vmSymbols::startRemoteAgent_name(), vmSymbols::string_void_signature(), str, CHECK);
}
void JvmtiClassFileReconstituter::write_class_file_format() {
ReallocMark();
// JVMSpec| ClassFile {
// JVMSpec| u4 magic;
write_u4(0xCAFEBABE);
// JVMSpec| u2 minor_version;
// JVMSpec| u2 major_version;
write_u2(ikh()->minor_version());
u2 major = ikh()->major_version();
write_u2(major);
// JVMSpec| u2 constant_pool_count;
// JVMSpec| cp_info constant_pool[constant_pool_count-1];
write_u2(cpool()->length());
copy_cpool_bytes(writeable_address(cpool_size()));
// JVMSpec| u2 access_flags;
write_u2(ikh()->access_flags().get_flags() & JVM_RECOGNIZED_CLASS_MODIFIERS);
// JVMSpec| u2 this_class;
// JVMSpec| u2 super_class;
write_u2(class_symbol_to_cpool_index(ikh()->name()));
Klass* super_class = ikh()->super();
write_u2(super_class == NULL? 0 : // zero for java.lang.Object
class_symbol_to_cpool_index(super_class->name()));
// JVMSpec| u2 interfaces_count;
// JVMSpec| u2 interfaces[interfaces_count];
Array<Klass*>* interfaces = ikh()->local_interfaces();
int num_interfaces = interfaces->length();
write_u2(num_interfaces);
for (int index = 0; index < num_interfaces; index++) {
HandleMark hm(thread());
instanceKlassHandle iikh(thread(), interfaces->at(index));
write_u2(class_symbol_to_cpool_index(iikh->name()));
}
// JVMSpec| u2 fields_count;
// JVMSpec| field_info fields[fields_count];
write_field_infos();
// JVMSpec| u2 methods_count;
// JVMSpec| method_info methods[methods_count];
write_method_infos();
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
// JVMSpec| } /* end ClassFile 8?
write_class_attributes();
}
static void loadAgentModule(TRAPS) {
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
JavaValue result(T_OBJECT);
Handle h_module_name = java_lang_String::create_from_str("jdk.management.agent", CHECK);
JavaCalls::call_static(&result,
SystemDictionary::module_Modules_klass(),
vmSymbols::loadModule_name(),
vmSymbols::loadModule_signature(),
h_module_name,
THREAD);
}
/** Main entry point. */
int main(int argc, char **argv)
{
ros::init(argc, argv, "heightmap_node");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
// create height map class, which subscribes to velodyne_points
velodyne_height_map::HeightMap hm(node, priv_nh);
// handle callbacks until shut down
ros::spin();
return 0;
}
cv::Mat pointsToMat(std::vector<cv::Point2d>& pts)
{
// each column of hm is a homogeneous coordinate of a point.
int c=pts.size(); // amount of points
cv::Mat hm(3,c,CV_64FC1,cv::Scalar(1.0));
cv::Mat m(pts);
m=m.reshape(1,c);
m=m.t();
cv::Mat roi(hm, cv::Rect(0,0,c,2));
m.copyTo(roi);
return hm;
}
void testMultiA(){
/**
* 多维数组数组第一维的元素实际上是另一个数组
* int matrix[3][0]可以看作是一个一维数组，包含三个元素，只是每个元素恰好是包含10个整型元素的数组。
* matrix这个名字的值是一个指向它的第一个元素的指针，所以matrix是一个指向一个包含10个整数元素的数组的指针。
* */
short m[2][3]={100,101,102,110,111,112};
short (*p)[3]=m;
printf("p:sizeof(p):%d,sizeof(p[0]):%d,sizeof(p[1]:%d\n",sizeof(p),sizeof(p[0]),sizeof(p[1]));
printf("sizeof(m):%d,sizeof(m[0]):%d,sizeof(m[1]:%d\n",sizeof(m),sizeof(m[0]),sizeof(m[1]));
printf("*((m+1)+1):%d,*(m[1]+1):%d,m[1][1]:%d\n",*(*(m+1)+1),*(m[1]+1),m[1][1]);
hm(m);
hm1(m);
}
void JMXStopRemoteDCmd::execute(DCmdSource source, TRAPS) {
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
// Load and initialize the sun.management.Agent class
// invoke stopRemoteManagementAgent method to stop the
// management server
// throw java.lang.NoSuchMethodError if method doesn't exist
Handle loader = Handle(THREAD, SystemDictionary::java_system_loader());
Klass* k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK);
instanceKlassHandle ik (THREAD, k);
JavaValue result(T_VOID);
JavaCalls::call_static(&result, ik, vmSymbols::stopRemoteAgent_name(), vmSymbols::void_method_signature(), CHECK);
}
double *iter2(double *y0, double x0, double h, double *y, int cnt)
{
int i;
double k1,k2,k3,k4;
double *tmp = malloc (cnt * sizeof(double));
for (i = 0; i < cnt; i++)
{
k1 = func[i](x0, y0);
k2 = func[i](x0 + h/2, hn(y0, cnt, h, k1, tmp));
k3 = func[i](x0 + h/2, hn(y0, cnt, h, k2, tmp));
k4 = func[i](x0 + h, hm(y0, cnt, h, k3, tmp));
y[i] = y0[i] + (k1 + 2 * k2 + 2 * k3 + k4) * h / 6;
}
free(tmp);
return y;
}
void LowMemoryDetector::process_sensor_changes(TRAPS) {
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
// No need to hold Service_lock to call out to Java
int num_memory_pools = MemoryService::num_memory_pools();
for (int i = 0; i < num_memory_pools; i++) {
MemoryPool* pool = MemoryService::get_memory_pool(i);
SensorInfo* sensor = pool->usage_sensor();
SensorInfo* gc_sensor = pool->gc_usage_sensor();
if (sensor != NULL && sensor->has_pending_requests()) {
sensor->process_pending_requests(CHECK);
}
if (gc_sensor != NULL && gc_sensor->has_pending_requests()) {
gc_sensor->process_pending_requests(CHECK);
}
}
}
void LowMemoryDetector::low_memory_detector_thread_entry(JavaThread* jt, TRAPS) {
while (true) {
bool sensors_changed = false;
{
// _no_safepoint_check_flag is used here as LowMemory_lock is a
// special lock and the VMThread may acquire this lock at safepoint.
// Need state transition ThreadBlockInVM so that this thread
// will be handled by safepoint correctly when this thread is
// notified at a safepoint.
// This ThreadBlockInVM object is not also considered to be
// suspend-equivalent because LowMemoryDetector threads are
// not visible to external suspension.
ThreadBlockInVM tbivm(jt);
MutexLockerEx ml(LowMemory_lock, Mutex::_no_safepoint_check_flag);
while (!(sensors_changed = has_pending_requests())) {
// wait until one of the sensors has pending requests
LowMemory_lock->wait(Mutex::_no_safepoint_check_flag);
}
}
{
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
// No need to hold LowMemory_lock to call out to Java
int num_memory_pools = MemoryService::num_memory_pools();
for (int i = 0; i < num_memory_pools; i++) {
MemoryPool* pool = MemoryService::get_memory_pool(i);
SensorInfo* sensor = pool->usage_sensor();
SensorInfo* gc_sensor = pool->gc_usage_sensor();
if (sensor != NULL && sensor->has_pending_requests()) {
sensor->process_pending_requests(CHECK);
}
if (gc_sensor != NULL && gc_sensor->has_pending_requests()) {
gc_sensor->process_pending_requests(CHECK);
}
}
}
}
}
// Write the method information portion of ClassFile structure
// JVMSpec| u2 methods_count;
// JVMSpec| method_info methods[methods_count];
void JvmtiClassFileReconstituter::write_method_infos() {
HandleMark hm(thread());
Array<Method*>* methods = ikh()->methods();
int num_methods = methods->length();
int num_overpass = 0;
// count the generated default interface methods
// these will not be re-created by write_method_info
// and should not be included in the total count
for (int index = 0; index < num_methods; index++) {
Method* method = methods->at(index);
if (method->is_overpass()) {
num_overpass++;
}
}
write_u2(num_methods - num_overpass);
if (JvmtiExport::can_maintain_original_method_order()) {
int index;
int original_index;
intArray method_order(num_methods, 0);
// invert the method order mapping
for (index = 0; index < num_methods; index++) {
original_index = ikh()->method_ordering()->at(index);
assert(original_index >= 0 && original_index < num_methods,
"invalid original method index");
method_order.at_put(original_index, index);
}
// write in original order
for (original_index = 0; original_index < num_methods; original_index++) {
index = method_order.at(original_index);
methodHandle method(thread(), methods->at(index));
write_method_info(method);
}
} else {
// method order not preserved just dump the method infos
for (int index = 0; index < num_methods; index++) {
methodHandle method(thread(), methods->at(index));
write_method_info(method);
}
}
}
void connection_handler::handle_accepts()
{
detail::handling_messages hm(handling_accepts_); // reset on exit
std::size_t k = 64;
while(!stopped_)
{
hpx::util::high_resolution_timer t;
boost::shared_ptr<receiver> rcv = acceptor_.accept(*this, memory_pool_, boost::system::throws);
if(rcv)
{
rcv->async_read(boost::system::throws);
{
hpx::lcos::local::spinlock::scoped_lock l(receivers_mtx_);
receivers_.push_back(rcv);
}
}
time_acct += t.elapsed();
hpx::lcos::local::spinlock::yield(k);
}
}
jvmtiError
JvmtiEnvBase::get_frame_location(JavaThread *java_thread, jint depth,
jmethodID* method_ptr, jlocation* location_ptr) {
#ifdef ASSERT
uint32_t debug_bits = 0;
#endif
assert((SafepointSynchronize::is_at_safepoint() ||
is_thread_fully_suspended(java_thread, false, &debug_bits)),
"at safepoint or target thread is suspended");
Thread* current_thread = Thread::current();
ResourceMark rm(current_thread);
vframe *vf = vframeFor(java_thread, depth);
if (vf == NULL) {
return JVMTI_ERROR_NO_MORE_FRAMES;
}
// vframeFor should return a java frame. If it doesn't
// it means we've got an internal error and we return the
// error in product mode. In debug mode we will instead
// attempt to cast the vframe to a javaVFrame and will
// cause an assertion/crash to allow further diagnosis.
#ifdef PRODUCT
if (!vf->is_java_frame()) {
return JVMTI_ERROR_INTERNAL;
}
#endif
HandleMark hm(current_thread);
javaVFrame *jvf = javaVFrame::cast(vf);
Method* method = jvf->method();
if (method->is_native()) {
*location_ptr = -1;
} else {
*location_ptr = jvf->bci();
}
*method_ptr = method->jmethod_id();
return JVMTI_ERROR_NONE;
}
void connection_handler::handle_messages()
{
detail::handling_messages hm(handling_messages_); // reset on exit
bool bootstrapping = hpx::is_starting();
bool has_work = true;
std::size_t k = 0;
hpx::util::high_resolution_timer t;
// We let the message handling loop spin for another 2 seconds to avoid the
// costs involved with posting it to asio
while(bootstrapping || (!stopped_ && has_work) || (!has_work && t.elapsed() < 2.0))
{
// handle all sends ...
has_work = do_sends();
// handle all receives ...
if(do_receives())
{
has_work = true;
}
if (bootstrapping)
bootstrapping = hpx::is_starting();
if(has_work)
{
t.restart();
k = 0;
}
else
{
if(enable_parcel_handling_)
{
hpx::lcos::local::spinlock::yield(k);
++k;
}
}
}
}
void GCNotifier::sendNotificationInternal(TRAPS) {
ResourceMark rm(THREAD);
HandleMark hm(THREAD);
GCNotificationRequest *request = getRequest();
if (request != NULL) {
NotificationMark nm(request);
Handle objGcInfo = createGcInfo(request->gcManager, request->gcStatInfo, THREAD);
Handle objName = java_lang_String::create_from_platform_dependent_str(request->gcManager->name(), CHECK);
Handle objAction = java_lang_String::create_from_platform_dependent_str(request->gcAction, CHECK);
Handle objCause = java_lang_String::create_from_platform_dependent_str(request->gcCause, CHECK);
Klass* k = Management::sun_management_GarbageCollectorImpl_klass(CHECK);
instanceKlassHandle gc_mbean_klass(THREAD, k);
instanceOop gc_mbean = request->gcManager->get_memory_manager_instance(THREAD);
instanceHandle gc_mbean_h(THREAD, gc_mbean);
if (!gc_mbean_h->is_a(k)) {
THROW_MSG(vmSymbols::java_lang_IllegalArgumentException(),
"This GCMemoryManager doesn't have a GarbageCollectorMXBean");
}
JavaValue result(T_VOID);
JavaCallArguments args(gc_mbean_h);
args.push_long(request->timestamp);
args.push_oop(objName);
args.push_oop(objAction);
args.push_oop(objCause);
args.push_oop(objGcInfo);
JavaCalls::call_virtual(&result,
gc_mbean_klass,
vmSymbols::createGCNotification_name(),
vmSymbols::createGCNotification_signature(),
&args,
CHECK);
}
}
void buf_flush()
{
int i;
if (strcmp(text[0], "") == 0) return;
switch (buf_type) {
case TYPE_TEXT:
printf("<p>\n");
break;
case TYPE_PRE:
printf("<pre>\n");
break;
case TYPE_LI:
printf("<ul>\n");
break;
}
for (i = 0; i < 100; i++) {
if (strcmp(text[i], "") == 0) break;
if (buf_type == TYPE_LI)
printf("<li>");
hm(text[i]);
o("\n");
if (buf_type == TYPE_LI)
printf("</li>");
}
switch (buf_type) {
case TYPE_TEXT:
printf("</p>\n");
break;
case TYPE_PRE:
printf("</pre>\n");
break;
case TYPE_LI:
printf("</ul>\n");
break;
}
buf_clear();
}
static void
ngx_nats_process_msg(ngx_nats_connection_t *nc, ngx_nats_buf_t *buf,
ngx_nats_msg_t *msg)
{
ngx_nats_subscription_t *sub;
ngx_nats_handle_msg_pt hm;
void *sub_data;
ngx_nats_client_t *client;
ngx_str_t *r = NULL;
ngx_int_t sid;
ngx_pool_t *pool;
ngx_nats_message_t *m;
sid = msg->sid;
sub = ngx_nats_get_subscription(nc, sid);
if (sub == NULL) {
return;
}
if (msg->replyto.len > 0) {
r = &msg->replyto;
}
/* we may remove subscription so cache these */
hm = sub->handle_msg;
sub_data = sub->client_subscription_data;
client = sub->client;
if (sub->max > 0) {
sub->recv++;
if (sub->recv >= sub->max) {
ngx_nats_remove_subscription(nc, sub->sid);
}
}
pool = ngx_create_pool(NGX_DEFAULT_POOL_SIZE, nc->nd->log);
if (pool == NULL) {
ngx_log_error(NGX_LOG_CRIT, nc->nd->log, 0,
"%s: ngx_create_pool failed", __func__);
return;
}
m = ngx_pcalloc(pool, sizeof(ngx_nats_message_t));
if (m == NULL) {
ngx_log_error(NGX_LOG_CRIT, nc->nd->log, 0,
"%s: ngx_pcalloc failed", __func__);
goto DONE;
}
m->client = client;
m->sid = sid;
m->subject = &msg->subject;
m->replyto = r;
m->pool = pool;
m->client_subscription_data = sub_data;
m->data.data = (u_char *) (buf->buf + buf->pos + msg->bstart);
m->data.len = msg->bend - msg->bstart;
hm(m);
DONE:
ngx_destroy_pool(pool);
}
static void find(intptr_t x, bool print_pc) {
address addr = (address)x;
CodeBlob* b = CodeCache::find_blob_unsafe(addr);
if (b != NULL) {
if (b->is_buffer_blob()) {
// the interpreter is generated into a buffer blob
InterpreterCodelet* i = Interpreter::codelet_containing(addr);
if (i != NULL) {
i->print();
return;
}
if (Interpreter::contains(addr)) {
tty->print_cr(INTPTR_FORMAT " is pointing into interpreter code (not bytecode specific)", addr);
return;
}
//
if (AdapterHandlerLibrary::contains(b)) {
AdapterHandlerLibrary::print_handler(b);
}
// the stubroutines are generated into a buffer blob
StubCodeDesc* d = StubCodeDesc::desc_for(addr);
if (d != NULL) {
d->print();
if (print_pc) tty->cr();
return;
}
if (StubRoutines::contains(addr)) {
tty->print_cr(INTPTR_FORMAT " is pointing to an (unnamed) stub routine", addr);
return;
}
// the InlineCacheBuffer is using stubs generated into a buffer blob
if (InlineCacheBuffer::contains(addr)) {
tty->print_cr(INTPTR_FORMAT " is pointing into InlineCacheBuffer", addr);
return;
}
VtableStub* v = VtableStubs::stub_containing(addr);
if (v != NULL) {
v->print();
return;
}
}
if (print_pc && b->is_nmethod()) {
ResourceMark rm;
tty->print("%#p: Compiled ", addr);
((nmethod*)b)->method()->print_value_on(tty);
tty->print(" = (CodeBlob*)" INTPTR_FORMAT, b);
tty->cr();
return;
}
if ( b->is_nmethod()) {
if (b->is_zombie()) {
tty->print_cr(INTPTR_FORMAT " is zombie nmethod", b);
} else if (b->is_not_entrant()) {
tty->print_cr(INTPTR_FORMAT " is non-entrant nmethod", b);
}
}
b->print();
return;
}
if (Universe::heap()->is_in(addr)) {
HeapWord* p = Universe::heap()->block_start(addr);
bool print = false;
// If we couldn't find it it just may mean that heap wasn't parseable
// See if we were just given an oop directly
if (p != NULL && Universe::heap()->block_is_obj(p)) {
print = true;
} else if (p == NULL && ((oopDesc*)addr)->is_oop()) {
p = (HeapWord*) addr;
print = true;
}
if (print) {
oop(p)->print();
if (p != (HeapWord*)x && oop(p)->is_constMethod() &&
constMethodOop(p)->contains(addr)) {
Thread *thread = Thread::current();
HandleMark hm(thread);
methodHandle mh (thread, constMethodOop(p)->method());
if (!mh->is_native()) {
tty->print_cr("bci_from(%p) = %d; print_codes():",
addr, mh->bci_from(address(x)));
mh->print_codes();
}
}
return;
}
} else if (Universe::heap()->is_in_reserved(addr)) {
tty->print_cr(INTPTR_FORMAT " is an unallocated location in the heap", addr);
return;
}
if (JNIHandles::is_global_handle((jobject) addr)) {
tty->print_cr(INTPTR_FORMAT " is a global jni handle", addr);
return;
}
if (JNIHandles::is_weak_global_handle((jobject) addr)) {
tty->print_cr(INTPTR_FORMAT " is a weak global jni handle", addr);
return;
}
if (JNIHandleBlock::any_contains((jobject) addr)) {
tty->print_cr(INTPTR_FORMAT " is a local jni handle", addr);
return;
}
for(JavaThread *thread = Threads::first(); thread; thread = thread->next()) {
// Check for privilege stack
if (thread->privileged_stack_top() != NULL && thread->privileged_stack_top()->contains(addr)) {
tty->print_cr(INTPTR_FORMAT " is pointing into the privilege stack for thread: " INTPTR_FORMAT, addr, thread);
return;
}
// If the addr is a java thread print information about that.
if (addr == (address)thread) {
thread->print();
return;
}
}
// Try an OS specific find
if (os::find(addr)) {
return;
}
if (print_pc) {
tty->print_cr(INTPTR_FORMAT ": probably in C++ code; check debugger", addr);
Disassembler::decode(same_page(addr-40,addr),same_page(addr+40,addr));
return;
}
tty->print_cr(INTPTR_FORMAT " is pointing to unknown location", addr);
}
int CppInterpreter::native_entry(methodOop method, intptr_t UNUSED, TRAPS) {
// Make sure method is native and not abstract
assert(method->is_native() && !method->is_abstract(), "should be");
JavaThread *thread = (JavaThread *) THREAD;
ZeroStack *stack = thread->zero_stack();
// Allocate and initialize our frame
InterpreterFrame *frame = InterpreterFrame::build(method, CHECK_0);
thread->push_zero_frame(frame);
interpreterState istate = frame->interpreter_state();
intptr_t *locals = istate->locals();
// Update the invocation counter
if ((UseCompiler || CountCompiledCalls) && !method->is_synchronized()) {
InvocationCounter *counter = method->invocation_counter();
counter->increment();
if (counter->reached_InvocationLimit()) {
CALL_VM_NOCHECK(
InterpreterRuntime::frequency_counter_overflow(thread, NULL));
if (HAS_PENDING_EXCEPTION)
goto unwind_and_return;
}
}
// Lock if necessary
BasicObjectLock *monitor;
monitor = NULL;
if (method->is_synchronized()) {
monitor = (BasicObjectLock*) istate->stack_base();
oop lockee = monitor->obj();
markOop disp = lockee->mark()->set_unlocked();
monitor->lock()->set_displaced_header(disp);
if (Atomic::cmpxchg_ptr(monitor, lockee->mark_addr(), disp) != disp) {
if (thread->is_lock_owned((address) disp->clear_lock_bits())) {
monitor->lock()->set_displaced_header(NULL);
}
else {
CALL_VM_NOCHECK(InterpreterRuntime::monitorenter(thread, monitor));
if (HAS_PENDING_EXCEPTION)
goto unwind_and_return;
}
}
}
// Get the signature handler
InterpreterRuntime::SignatureHandler *handler; {
address handlerAddr = method->signature_handler();
if (handlerAddr == NULL) {
CALL_VM_NOCHECK(InterpreterRuntime::prepare_native_call(thread, method));
if (HAS_PENDING_EXCEPTION)
goto unlock_unwind_and_return;
handlerAddr = method->signature_handler();
assert(handlerAddr != NULL, "eh?");
}
if (handlerAddr == (address) InterpreterRuntime::slow_signature_handler) {
CALL_VM_NOCHECK(handlerAddr =
InterpreterRuntime::slow_signature_handler(thread, method, NULL,NULL));
if (HAS_PENDING_EXCEPTION)
goto unlock_unwind_and_return;
}
handler = \
InterpreterRuntime::SignatureHandler::from_handlerAddr(handlerAddr);
}
// Get the native function entry point
address function;
function = method->native_function();
assert(function != NULL, "should be set if signature handler is");
// Build the argument list
stack->overflow_check(handler->argument_count() * 2, THREAD);
if (HAS_PENDING_EXCEPTION)
goto unlock_unwind_and_return;
void **arguments;
void *mirror; {
arguments =
(void **) stack->alloc(handler->argument_count() * sizeof(void **));
void **dst = arguments;
void *env = thread->jni_environment();
*(dst++) = &env;
if (method->is_static()) {
istate->set_oop_temp(
method->constants()->pool_holder()->java_mirror());
mirror = istate->oop_temp_addr();
*(dst++) = &mirror;
}
intptr_t *src = locals;
for (int i = dst - arguments; i < handler->argument_count(); i++) {
ffi_type *type = handler->argument_type(i);
if (type == &ffi_type_pointer) {
if (*src) {
stack->push((intptr_t) src);
*(dst++) = stack->sp();
}
else {
*(dst++) = src;
}
src--;
}
else if (type->size == 4) {
*(dst++) = src--;
}
else if (type->size == 8) {
src--;
*(dst++) = src--;
}
else {
ShouldNotReachHere();
}
}
}
// Set up the Java frame anchor
thread->set_last_Java_frame();
// Change the thread state to _thread_in_native
ThreadStateTransition::transition_from_java(thread, _thread_in_native);
// Make the call
intptr_t result[4 - LogBytesPerWord];
ffi_call(handler->cif(), (void (*)()) function, result, arguments);
// Change the thread state back to _thread_in_Java.
// ThreadStateTransition::transition_from_native() cannot be used
// here because it does not check for asynchronous exceptions.
// We have to manage the transition ourself.
thread->set_thread_state(_thread_in_native_trans);
// Make sure new state is visible in the GC thread
if (os::is_MP()) {
if (UseMembar) {
OrderAccess::fence();
}
else {
InterfaceSupport::serialize_memory(thread);
}
}
// Handle safepoint operations, pending suspend requests,
// and pending asynchronous exceptions.
if (SafepointSynchronize::do_call_back() ||
thread->has_special_condition_for_native_trans()) {
JavaThread::check_special_condition_for_native_trans(thread);
CHECK_UNHANDLED_OOPS_ONLY(thread->clear_unhandled_oops());
}
// Finally we can change the thread state to _thread_in_Java.
thread->set_thread_state(_thread_in_Java);
fixup_after_potential_safepoint();
// Clear the frame anchor
thread->reset_last_Java_frame();
// If the result was an oop then unbox it and store it in
// oop_temp where the garbage collector can see it before
// we release the handle it might be protected by.
if (handler->result_type() == &ffi_type_pointer) {
if (result[0])
istate->set_oop_temp(*(oop *) result[0]);
else
istate->set_oop_temp(NULL);
}
// Reset handle block
thread->active_handles()->clear();
unlock_unwind_and_return:
// Unlock if necessary
if (monitor) {
BasicLock *lock = monitor->lock();
markOop header = lock->displaced_header();
oop rcvr = monitor->obj();
monitor->set_obj(NULL);
if (header != NULL) {
if (Atomic::cmpxchg_ptr(header, rcvr->mark_addr(), lock) != lock) {
monitor->set_obj(rcvr); {
HandleMark hm(thread);
CALL_VM_NOCHECK(InterpreterRuntime::monitorexit(thread, monitor));
}
}
}
}
unwind_and_return:
// Unwind the current activation
thread->pop_zero_frame();
// Pop our parameters
stack->set_sp(stack->sp() + method->size_of_parameters());
// Push our result
if (!HAS_PENDING_EXCEPTION) {
BasicType type = result_type_of(method);
stack->set_sp(stack->sp() - type2size[type]);
switch (type) {
case T_VOID:
break;
case T_BOOLEAN:
#ifndef VM_LITTLE_ENDIAN
result[0] <<= (BitsPerWord - BitsPerByte);
#endif
SET_LOCALS_INT(*(jboolean *) result != 0, 0);
break;
case T_CHAR:
#ifndef VM_LITTLE_ENDIAN
result[0] <<= (BitsPerWord - BitsPerShort);
#endif
SET_LOCALS_INT(*(jchar *) result, 0);
break;
case T_BYTE:
#ifndef VM_LITTLE_ENDIAN
result[0] <<= (BitsPerWord - BitsPerByte);
#endif
SET_LOCALS_INT(*(jbyte *) result, 0);
break;
case T_SHORT:
#ifndef VM_LITTLE_ENDIAN
result[0] <<= (BitsPerWord - BitsPerShort);
#endif
SET_LOCALS_INT(*(jshort *) result, 0);
break;
case T_INT:
#ifndef VM_LITTLE_ENDIAN
result[0] <<= (BitsPerWord - BitsPerInt);
#endif
SET_LOCALS_INT(*(jint *) result, 0);
break;
case T_LONG:
SET_LOCALS_LONG(*(jlong *) result, 0);
break;
case T_FLOAT:
SET_LOCALS_FLOAT(*(jfloat *) result, 0);
break;
case T_DOUBLE:
SET_LOCALS_DOUBLE(*(jdouble *) result, 0);
break;
case T_OBJECT:
case T_ARRAY:
SET_LOCALS_OBJECT(istate->oop_temp(), 0);
break;
default:
ShouldNotReachHere();
}
}
// No deoptimized frames on the stack
return 0;
}
void connection_handler::handle_messages()
{
detail::handling_messages hm(handling_messages_); // reset on exit
bool bootstrapping = hpx::is_starting();
bool has_work = true;
std::size_t k = 0;
hpx::util::high_resolution_timer t;
std::list<std::pair<int, MPI_Request> > close_requests;
// We let the message handling loop spin for another 2 seconds to avoid the
// costs involved with posting it to asio
while(bootstrapping || has_work || (!has_work && t.elapsed() < 2.0))
{
if(stopped_) break;
// break the loop if someone requested to pause the parcelport
if(!enable_parcel_handling_) break;
// handle all send requests
{
hpx::lcos::local::spinlock::scoped_lock l(senders_mtx_);
for(
senders_type::iterator it = senders_.begin();
!stopped_ && enable_parcel_handling_ && it != senders_.end();
/**/)
{
if((*it)->done())
{
it = senders_.erase(it);
}
else
{
++it;
}
}
has_work = !senders_.empty();
}
// Send the pending close requests
{
hpx::lcos::local::spinlock::scoped_lock l(close_mtx_);
typedef std::pair<int, int> pair_type;
BOOST_FOREACH(pair_type p, pending_close_requests_)
{
header close_request = header::close(p.first, p.second);
close_requests.push_back(std::make_pair(p.first, MPI_Request()));
MPI_Isend(
close_request.data(), // Data pointer
close_request.data_size_, // Size
close_request.type(), // MPI Datatype
close_request.rank(), // Destination
0, // Tag
communicator_, // Communicator
&close_requests.back().second
);
}
pending_close_requests_.clear();
}
// add new receive requests
std::pair<bool, header> next(acceptor_.next_header());
if(next.first)
{
boost::shared_ptr<receiver> rcv;
header h = next.second;
receivers_tag_map_type & tag_map = receivers_map_[h.rank()];
receivers_tag_map_type::iterator jt = tag_map.find(h.tag());
if(jt != tag_map.end())
{
rcv = jt->second;
}
else
{
rcv = boost::make_shared<receiver>(
communicator_
, get_next_tag()
, h.tag()
, h.rank()
, *this);
tag_map.insert(std::make_pair(h.tag(), rcv));
}
if(h.close_request())
{
rcv->close();
}
else
{
h.assert_valid();
if (static_cast<std::size_t>(h.size()) > this->get_max_message_size())
{
// report this problem ...
HPX_THROW_EXCEPTION(boost::asio::error::operation_not_supported,
"mpi::connection_handler::handle_messages",
"The size of this message exceeds the maximum inbound data size");
return;
}
if(rcv->async_read(h))
{
#ifdef HPX_DEBUG
receivers_type::iterator it = std::find(receivers_.begin(), receivers_.end(), rcv);
HPX_ASSERT(it == receivers_.end());
#endif
receivers_.push_back(rcv);
}
}
}
// handle all receive requests
for(receivers_type::iterator it = receivers_.begin();
it != receivers_.end(); /**/)
{
boost::shared_ptr<receiver> rcv = *it;
if(rcv->done())
{
HPX_ASSERT(rcv->sender_tag() != -1);
if(rcv->closing())
{
receivers_tag_map_type & tag_map = receivers_map_[rcv->rank()];
receivers_tag_map_type::iterator jt = tag_map.find(rcv->sender_tag());
HPX_ASSERT(jt != tag_map.end());
tag_map.erase(jt);
{
hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
free_tags_.push_back(rcv->tag());
}
}
it = receivers_.erase(it);
}
else
{
++it;
}
}
if(!has_work) has_work = !receivers_.empty();
// handle completed close requests
for(
std::list<std::pair<int, MPI_Request> >::iterator it = close_requests.begin();
!stopped_ && enable_parcel_handling_ && it != close_requests.end();
)
{
int completed = 0;
MPI_Status status;
int ret = 0;
ret = MPI_Test(&it->second, &completed, &status);
HPX_ASSERT(ret == MPI_SUCCESS);
if(completed && status.MPI_ERROR != MPI_ERR_PENDING)
{
hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
free_tags_.push_back(it->first);
it = close_requests.erase(it);
}
else
{
++it;
}
}
if(!has_work)
has_work = !close_requests.empty();
if (bootstrapping)
bootstrapping = hpx::is_starting();
if(has_work)
{
t.restart();
k = 0;
}
else
{
if(enable_parcel_handling_)
{
hpx::lcos::local::spinlock::yield(k);
++k;
}
}
}
void GravityColumnSolverPolymer<FluxModel, Model>::solveSingleColumn(const std::vector<int>& column_cells,
const double dt,
std::vector<double>& s,
std::vector<double>& c,
std::vector<double>& cmax,
std::vector<double>& sol_vec)
{
// This is written only to work with SinglePointUpwindTwoPhase,
// not with arbitrary problem models.
int col_size = column_cells.size();
// if (col_size == 1) {
// sol_vec[2*column_cells[0] + 0] = 0.0;
// sol_vec[2*column_cells[0] + 1] = 0.0;
// return;
// }
StateWithZeroFlux state(s, c, cmax); // This holds s by reference.
// Assemble.
const int kl = 3;
const int ku = 3;
const int nrow = 2*kl + ku + 1;
const int N = 2*col_size; // N unknowns: s and c for each cell.
std::vector<double> hm(nrow*N, 0.0); // band matrix with 3 upper and 3 lower diagonals.
std::vector<double> rhs(N, 0.0);
const BandMatrixCoeff bmc(N, ku, kl);
for (int ci = 0; ci < col_size; ++ci) {
std::vector<double> F(2, 0.);
std::vector<double> dFd1(4, 0.);
std::vector<double> dFd2(4, 0.);
std::vector<double> dF(4, 0.);
const int cell = column_cells[ci];
const int prev_cell = (ci == 0) ? -999 : column_cells[ci - 1];
const int next_cell = (ci == col_size - 1) ? -999 : column_cells[ci + 1];
// model_.initResidual(cell, F);
for (int j = grid_.cell_facepos[cell]; j < grid_.cell_facepos[cell+1]; ++j) {
const int face = grid_.cell_faces[j];
const int c1 = grid_.face_cells[2*face + 0];
const int c2 = grid_.face_cells[2*face + 1];
if (c1 == prev_cell || c2 == prev_cell || c1 == next_cell || c2 == next_cell) {
F.assign(2, 0.);
dFd1.assign(4, 0.);
dFd2.assign(4, 0.);
fmodel_.fluxConnection(state, grid_, dt, cell, face, &F[0], &dFd1[0], &dFd2[0]);
if (c1 == prev_cell || c2 == prev_cell) {
hm[bmc(2*ci + 0, 2*(ci - 1) + 0)] += dFd2[0];
hm[bmc(2*ci + 0, 2*(ci - 1) + 1)] += dFd2[1];
hm[bmc(2*ci + 1, 2*(ci - 1) + 0)] += dFd2[2];
hm[bmc(2*ci + 1, 2*(ci - 1) + 1)] += dFd2[3];
} else {
ASSERT(c1 == next_cell || c2 == next_cell);
hm[bmc(2*ci + 0, 2*(ci + 1) + 0)] += dFd2[0];
hm[bmc(2*ci + 0, 2*(ci + 1) + 1)] += dFd2[1];
hm[bmc(2*ci + 1, 2*(ci + 1) + 0)] += dFd2[2];
hm[bmc(2*ci + 1, 2*(ci + 1) + 1)] += dFd2[3];
}
hm[bmc(2*ci + 0, 2*ci + 0)] += dFd1[0];
hm[bmc(2*ci + 0, 2*ci + 1)] += dFd1[1];
hm[bmc(2*ci + 1, 2*ci + 0)] += dFd1[2];
hm[bmc(2*ci + 1, 2*ci + 1)] += dFd1[3];
rhs[2*ci + 0] += F[0];
rhs[2*ci + 1] += F[1];
}
}
F.assign(2, 0.);
dF.assign(4, 0.);
fmodel_.accumulation(grid_, cell, &F[0], &dF[0]);
hm[bmc(2*ci + 0, 2*ci + 0)] += dF[0];
hm[bmc(2*ci + 0, 2*ci + 1)] += dF[1];
hm[bmc(2*ci + 1, 2*ci + 0)] += dF[2];
if (std::abs(dF[3]) < 1e-12) {
hm[bmc(2*ci + 1, 2*ci + 1)] += 1e-12;
} else {
hm[bmc(2*ci + 1, 2*ci + 1)] += dF[3];
}
rhs[2*ci + 0] += F[0];
rhs[2*ci + 1] += F[1];
}
// model_.sourceTerms(); // Not needed
// Solve.
const int num_rhs = 1;
int info = 0;
std::vector<int> ipiv(N, 0);
// Solution will be written to rhs.
dgbsv_(&N, &kl, &ku, &num_rhs, &hm[0], &nrow, &ipiv[0], &rhs[0], &N, &info);
if (info != 0) {
std::cerr << "Failed column cells: ";
std::copy(column_cells.begin(), column_cells.end(), std::ostream_iterator<int>(std::cerr, " "));
std::cerr << "\n";
THROW("Lapack reported error in dgtsv: " << info);
}
for (int ci = 0; ci < col_size; ++ci) {
sol_vec[2*column_cells[ci] + 0] = -rhs[2*ci + 0];
sol_vec[2*column_cells[ci] + 1] = -rhs[2*ci + 1];
}
}
int main(int argc, char** argv) {
const double PI(3.141592653589793);
if (argc != 4) {
std::cout << "usage:" << std::endl;
std::cout << "package_scene path_scene output" << std::endl;
return 0;
}
ros::init(argc, argv, "dart_test");
ros::NodeHandle nh_;
std::string package_name( argv[1] );
std::string scene_urdf( argv[2] );
ros::Rate loop_rate(400);
ros::Publisher joint_state_pub_;
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 10);
tf::TransformBroadcaster br;
MarkerPublisher markers_pub(nh_);
std::string package_path_barrett = ros::package::getPath("barrett_hand_defs");
std::string package_path = ros::package::getPath(package_name);
// Load the Skeleton from a file
dart::utils::DartLoader loader;
loader.addPackageDirectory("barrett_hand_defs", package_path_barrett);
loader.addPackageDirectory("barrett_hand_sim_dart", package_path);
boost::shared_ptr<GraspSpecification > gspec = GraspSpecification::readFromUrdf(package_path + scene_urdf);
dart::dynamics::SkeletonPtr scene( loader.parseSkeleton(package_path + scene_urdf) );
scene->enableSelfCollision(true);
dart::dynamics::SkeletonPtr bh( loader.parseSkeleton(package_path_barrett + "/robots/barrett_hand.urdf") );
Eigen::Isometry3d tf;
tf = scene->getBodyNode("gripper_mount_link")->getRelativeTransform();
bh->getJoint(0)->setTransformFromParentBodyNode(tf);
dart::simulation::World* world = new dart::simulation::World();
world->addSkeleton(scene);
world->addSkeleton(bh);
Eigen::Vector3d grav(0,0,-1);
world->setGravity(grav);
GripperController gc;
double Kc = 400.0;
double KcDivTi = Kc / 1.0;
gc.addJoint("right_HandFingerOneKnuckleOneJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, true, false);
gc.addJoint("right_HandFingerOneKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJoint("right_HandFingerTwoKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJoint("right_HandFingerThreeKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJointMimic("right_HandFingerTwoKnuckleOneJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, true, "right_HandFingerOneKnuckleOneJoint", 1.0, 0.0);
gc.addJointMimic("right_HandFingerOneKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerOneKnuckleTwoJoint", 0.333333, 0.0);
gc.addJointMimic("right_HandFingerTwoKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerTwoKnuckleTwoJoint", 0.333333, 0.0);
gc.addJointMimic("right_HandFingerThreeKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerThreeKnuckleTwoJoint", 0.333333, 0.0);
gc.setGoalPosition("right_HandFingerOneKnuckleOneJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleOneJoint"));
gc.setGoalPosition("right_HandFingerOneKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleTwoJoint"));
gc.setGoalPosition("right_HandFingerTwoKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerTwoKnuckleTwoJoint"));
gc.setGoalPosition("right_HandFingerThreeKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerThreeKnuckleTwoJoint"));
std::map<std::string, double> joint_q_map;
joint_q_map["right_HandFingerOneKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
joint_q_map["right_HandFingerTwoKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
joint_q_map["right_HandFingerOneKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
joint_q_map["right_HandFingerOneKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
joint_q_map["right_HandFingerTwoKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
joint_q_map["right_HandFingerTwoKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
joint_q_map["right_HandFingerThreeKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
joint_q_map["right_HandFingerThreeKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
for (std::vector<std::string >::const_iterator it = gc.getJointNames().begin(); it != gc.getJointNames().end(); it++) {
dart::dynamics::Joint *j = bh->getJoint((*it));
j->setActuatorType(dart::dynamics::Joint::FORCE);
j->setPositionLimited(true);
j->setPosition(0, joint_q_map[(*it)]);
}
int counter = 0;
while (ros::ok()) {
world->step(false);
for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
dart::dynamics::Joint *j = bh->getJoint(it->first);
it->second = j->getPosition(0);
}
gc.controlStep(joint_q_map);
// Compute the joint forces needed to compensate for Coriolis forces and
// gravity
const Eigen::VectorXd& Cg = bh->getCoriolisAndGravityForces();
for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
dart::dynamics::Joint *j = bh->getJoint(it->first);
int qidx = j->getIndexInSkeleton(0);
double u = gc.getControl(it->first);
double dq = j->getVelocity(0);
if (!gc.isBackdrivable(it->first)) {
j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
}
if (gc.isStopped(it->first)) {
j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
j->setPositionUpperLimit(0, std::min(j->getPositionUpperLimit(0), it->second+0.01));
// std::cout << it->first << " " << "stopped" << std::endl;
}
j->setForce(0, 0.02*(u-dq) + Cg(qidx));
}
for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = bh->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
// std::cout << b->getName() << std::endl;
publishTransform(br, T_W_L, b->getName(), "world");
}
int m_id = 0;
for (int bidx = 0; bidx < scene->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = scene->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
publishTransform(br, T_W_L, b->getName(), "world");
for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
m_id = markers_pub.addMeshMarker(m_id, KDL::Vector(), 0, 1, 0, 1, 1, 1, 1, msh->getMeshUri(), b->getName());
}
}
}
markers_pub.publish();
ros::spinOnce();
loop_rate.sleep();
counter++;
if (counter < 3000) {
}
else if (counter == 3000) {
dart::dynamics::Joint::Properties prop = bh->getJoint(0)->getJointProperties();
dart::dynamics::FreeJoint::Properties prop_free;
prop_free.mName = prop_free.mName;
prop_free.mT_ParentBodyToJoint = prop.mT_ParentBodyToJoint;
prop_free.mT_ChildBodyToJoint = prop.mT_ChildBodyToJoint;
prop_free.mIsPositionLimited = false;
prop_free.mActuatorType = dart::dynamics::Joint::VELOCITY;
bh->getRootBodyNode()->changeParentJointType<dart::dynamics::FreeJoint >(prop_free);
}
else if (counter < 4000) {
bh->getDof("Joint_pos_z")->setVelocity(-0.1);
}
else {
break;
}
}
//
// generate models
//
const std::string ob_name( "graspable" );
scene->getBodyNode(ob_name)->setFrictionCoeff(0.001);
// calculate point clouds for all links and for the grasped object
std::map<std::string, pcl::PointCloud<pcl::PointNormal>::Ptr > point_clouds_map;
std::map<std::string, pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr > point_pc_clouds_map;
std::map<std::string, KDL::Frame > frames_map;
std::map<std::string, boost::shared_ptr<std::vector<KDL::Frame > > > features_map;
std::map<std::string, boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > > grids_map;
for (int skidx = 0; skidx < world->getNumSkeletons(); skidx++) {
dart::dynamics::SkeletonPtr sk = world->getSkeleton(skidx);
for (int bidx = 0; bidx < sk->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = sk->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
const std::string &body_name = b->getName();
if (body_name.find("right_Hand") != 0 && body_name != ob_name) {
continue;
}
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
std::cout << body_name << " " << b->getNumCollisionShapes() << std::endl;
for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
std::cout << "mesh path: " << msh->getMeshPath() << std::endl;
std::cout << "mesh uri: " << msh->getMeshUri() << std::endl;
const Eigen::Isometry3d &tf = sh->getLocalTransform();
KDL::Frame T_L_S;
EigenTfToKDL(tf, T_L_S);
KDL::Frame T_S_L = T_L_S.Inverse();
const aiScene *sc = msh->getMesh();
if (sc->mNumMeshes != 1) {
std::cout << "ERROR: sc->mNumMeshes = " << sc->mNumMeshes << std::endl;
}
int midx = 0;
// std::cout << "v: " << sc->mMeshes[midx]->mNumVertices << " f: " << sc->mMeshes[midx]->mNumFaces << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointNormal>);
uniform_sampling(sc->mMeshes[midx], 1000000, *cloud_1);
for (int pidx = 0; pidx < cloud_1->points.size(); pidx++) {
KDL::Vector pt_L = T_L_S * KDL::Vector(cloud_1->points[pidx].x, cloud_1->points[pidx].y, cloud_1->points[pidx].z);
cloud_1->points[pidx].x = pt_L.x();
cloud_1->points[pidx].y = pt_L.y();
cloud_1->points[pidx].z = pt_L.z();
}
// Voxelgrid
boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > grid_(new pcl::VoxelGrid<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr res(new pcl::PointCloud<pcl::PointNormal>);
grid_->setDownsampleAllData(true);
grid_->setSaveLeafLayout(true);
grid_->setInputCloud(cloud_1);
grid_->setLeafSize(0.004, 0.004, 0.004);
grid_->filter (*res);
point_clouds_map[body_name] = res;
frames_map[body_name] = T_W_L;
grids_map[body_name] = grid_;
std::cout << "res->points.size(): " << res->points.size() << std::endl;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>);
// Setup the principal curvatures computation
pcl::PrincipalCurvaturesEstimation<pcl::PointNormal, pcl::PointNormal, pcl::PrincipalCurvatures> principalCurvaturesEstimation;
// Provide the original point cloud (without normals)
principalCurvaturesEstimation.setInputCloud (res);
// Provide the point cloud with normals
principalCurvaturesEstimation.setInputNormals(res);
// Use the same KdTree from the normal estimation
principalCurvaturesEstimation.setSearchMethod (tree);
principalCurvaturesEstimation.setRadiusSearch(0.02);
// Actually compute the principal curvatures
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principalCurvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
principalCurvaturesEstimation.compute (*principalCurvatures);
point_pc_clouds_map[body_name] = principalCurvatures;
features_map[body_name].reset( new std::vector<KDL::Frame >(res->points.size()) );
for (int pidx = 0; pidx < res->points.size(); pidx++) {
KDL::Vector nx, ny, nz(res->points[pidx].normal[0], res->points[pidx].normal[1], res->points[pidx].normal[2]);
if ( std::fabs( principalCurvatures->points[pidx].pc1 - principalCurvatures->points[pidx].pc2 ) > 0.001) {
nx = KDL::Vector(principalCurvatures->points[pidx].principal_curvature[0], principalCurvatures->points[pidx].principal_curvature[1], principalCurvatures->points[pidx].principal_curvature[2]);
}
else {
if (std::fabs(nz.z()) < 0.7) {
nx = KDL::Vector(0, 0, 1);
}
else {
nx = KDL::Vector(1, 0, 0);
}
}
ny = nz * nx;
nx = ny * nz;
nx.Normalize();
ny.Normalize();
nz.Normalize();
(*features_map[body_name])[pidx] = KDL::Frame( KDL::Rotation(nx, ny, nz), KDL::Vector(res->points[pidx].x, res->points[pidx].y, res->points[pidx].z) );
}
}
}
}
}
const double sigma_p = 0.01;//05;
const double sigma_q = 10.0/180.0*PI;//100.0;
const double sigma_r = 0.2;//05;
double sigma_c = 5.0/180.0*PI;
int m_id = 101;
// generate object model
boost::shared_ptr<ObjectModel > om(new ObjectModel);
for (int pidx = 0; pidx < point_clouds_map[ob_name]->points.size(); pidx++) {
if (point_pc_clouds_map[ob_name]->points[pidx].pc1 > 1.1 * point_pc_clouds_map[ob_name]->points[pidx].pc2) {
// e.g. pc1=1, pc2=0
// edge
om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(PI)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
om->addPointFeature((*features_map[ob_name])[pidx], point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
}
else {
for (double angle = 0.0; angle < 359.0/180.0*PI; angle += 20.0/180.0*PI) {
om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(angle)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
}
}
}
std::cout << "om.getPointFeatures().size(): " << om->getPointFeatures().size() << std::endl;
KDL::Frame T_W_O = frames_map[ob_name];
// generate collision model
std::map<std::string, std::list<std::pair<int, double> > > link_pt_map;
boost::shared_ptr<CollisionModel > cm(new CollisionModel);
cm->setSamplerParameters(sigma_p, sigma_q, sigma_r);
std::list<std::string > gripper_link_names;
for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
const std::string &link_name = bh->getBodyNode(bidx)->getName();
gripper_link_names.push_back(link_name);
}
double dist_range = 0.01;
for (std::list<std::string >::const_iterator nit = gripper_link_names.begin(); nit != gripper_link_names.end(); nit++) {
const std::string &link_name = (*nit);
if (point_clouds_map.find( link_name ) == point_clouds_map.end()) {
continue;
}
cm->addLinkContacts(dist_range, link_name, point_clouds_map[link_name], frames_map[link_name],
om->getPointFeatures(), T_W_O);
}
// generate hand configuration model
boost::shared_ptr<HandConfigurationModel > hm(new HandConfigurationModel);
std::map<std::string, double> joint_q_map_before( joint_q_map );
double angleDiffKnuckleTwo = 15.0/180.0*PI;
joint_q_map_before["right_HandFingerOneKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerTwoKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerThreeKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerOneKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
joint_q_map_before["right_HandFingerTwoKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
joint_q_map_before["right_HandFingerThreeKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
hm->generateModel(joint_q_map_before, joint_q_map, 1.0, 10, sigma_c);
writeToXml(argv[3], cm, hm);
return 0;
}
jvmtiError
JvmtiEnvBase::get_stack_trace(JavaThread *java_thread,
jint start_depth, jint max_count,
jvmtiFrameInfo* frame_buffer, jint* count_ptr) {
#ifdef ASSERT
uint32_t debug_bits = 0;
#endif
assert((SafepointSynchronize::is_at_safepoint() ||
is_thread_fully_suspended(java_thread, false, &debug_bits)),
"at safepoint or target thread is suspended");
int count = 0;
if (java_thread->has_last_Java_frame()) {
RegisterMap reg_map(java_thread);
Thread* current_thread = Thread::current();
ResourceMark rm(current_thread);
javaVFrame *jvf = java_thread->last_java_vframe(®_map);
HandleMark hm(current_thread);
if (start_depth != 0) {
if (start_depth > 0) {
for (int j = 0; j < start_depth && jvf != NULL; j++) {
jvf = jvf->java_sender();
}
if (jvf == NULL) {
// start_depth is deeper than the stack depth
return JVMTI_ERROR_ILLEGAL_ARGUMENT;
}
} else { // start_depth < 0
// we are referencing the starting depth based on the oldest
// part of the stack.
// optimize to limit the number of times that java_sender() is called
javaVFrame *jvf_cursor = jvf;
javaVFrame *jvf_prev = NULL;
javaVFrame *jvf_prev_prev;
int j = 0;
while (jvf_cursor != NULL) {
jvf_prev_prev = jvf_prev;
jvf_prev = jvf_cursor;
for (j = 0; j > start_depth && jvf_cursor != NULL; j--) {
jvf_cursor = jvf_cursor->java_sender();
}
}
if (j == start_depth) {
// previous pointer is exactly where we want to start
jvf = jvf_prev;
} else {
// we need to back up further to get to the right place
if (jvf_prev_prev == NULL) {
// the -start_depth is greater than the stack depth
return JVMTI_ERROR_ILLEGAL_ARGUMENT;
}
// j now is the number of frames on the stack starting with
// jvf_prev, we start from jvf_prev_prev and move older on
// the stack that many, the result is -start_depth frames
// remaining.
jvf = jvf_prev_prev;
for (; j < 0; j++) {
jvf = jvf->java_sender();
}
}
}
}
for (; count < max_count && jvf != NULL; count++) {
frame_buffer[count].method = jvf->method()->jmethod_id();
frame_buffer[count].location = (jvf->method()->is_native() ? -1 : jvf->bci());
jvf = jvf->java_sender();
}
} else {
if (start_depth != 0) {
// no frames and there is a starting depth
return JVMTI_ERROR_ILLEGAL_ARGUMENT;
}
}
*count_ptr = count;
return JVMTI_ERROR_NONE;
}
// Write the field information portion of ClassFile structure
// JVMSpec| u2 fields_count;
// JVMSpec| field_info fields[fields_count];
void JvmtiClassFileReconstituter::write_field_infos() {
HandleMark hm(thread());
typeArrayHandle fields(thread(), ikh()->fields());
int fields_length = fields->length();
int num_fields = fields_length / instanceKlass::next_offset;
objArrayHandle fields_anno(thread(), ikh()->fields_annotations());
write_u2(num_fields);
for (int index = 0; index < fields_length; index += instanceKlass::next_offset) {
AccessFlags access_flags;
int flags = fields->ushort_at(index + instanceKlass::access_flags_offset);
access_flags.set_flags(flags);
int name_index = fields->ushort_at(index + instanceKlass::name_index_offset);
int signature_index = fields->ushort_at(index + instanceKlass::signature_index_offset);
int initial_value_index = fields->ushort_at(index + instanceKlass::initval_index_offset);
guarantee(name_index != 0 && signature_index != 0, "bad constant pool index for field");
int offset = ikh()->offset_from_fields( index );
int generic_signature_index =
fields->ushort_at(index + instanceKlass::generic_signature_offset);
typeArrayHandle anno(thread(), fields_anno.not_null() ?
(typeArrayOop)(fields_anno->obj_at(index / instanceKlass::next_offset)) :
(typeArrayOop)NULL);
// JVMSpec| field_info {
// JVMSpec| u2 access_flags;
// JVMSpec| u2 name_index;
// JVMSpec| u2 descriptor_index;
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
// JVMSpec| }
write_u2(flags & JVM_RECOGNIZED_FIELD_MODIFIERS);
write_u2(name_index);
write_u2(signature_index);
int attr_count = 0;
if (initial_value_index != 0) {
++attr_count;
}
if (access_flags.is_synthetic()) {
// ++attr_count;
}
if (generic_signature_index != 0) {
++attr_count;
}
if (anno.not_null()) {
++attr_count; // has RuntimeVisibleAnnotations attribute
}
write_u2(attr_count);
if (initial_value_index != 0) {
write_attribute_name_index("ConstantValue");
write_u4(2); //length always 2
write_u2(initial_value_index);
}
if (access_flags.is_synthetic()) {
// write_synthetic_attribute();
}
if (generic_signature_index != 0) {
write_signature_attribute(generic_signature_index);
}
if (anno.not_null()) {
write_annotations_attribute("RuntimeVisibleAnnotations", anno);
}
}
}
// Revised lookup semantics introduced 1.3 (Kestral beta)
void klassVtable::initialize_vtable(bool checkconstraints, TRAPS) {
// Note: Arrays can have intermediate array supers. Use java_super to skip them.
KlassHandle super (THREAD, klass()->java_super());
int nofNewEntries = 0;
if (PrintVtables && !klass()->oop_is_array()) {
ResourceMark rm(THREAD);
tty->print_cr("Initializing: %s", _klass->name()->as_C_string());
}
#ifdef ASSERT
oop* end_of_obj = (oop*)_klass() + _klass()->size();
oop* end_of_vtable = (oop*)&table()[_length];
assert(end_of_vtable <= end_of_obj, "vtable extends beyond end");
#endif
if (Universe::is_bootstrapping()) {
// just clear everything
for (int i = 0; i < _length; i++) table()[i].clear();
return;
}
int super_vtable_len = initialize_from_super(super);
if (klass()->oop_is_array()) {
assert(super_vtable_len == _length, "arrays shouldn't introduce new methods");
} else {
assert(_klass->oop_is_instance(), "must be instanceKlass");
objArrayHandle methods(THREAD, ik()->methods());
int len = methods()->length();
int initialized = super_vtable_len;
// update_inherited_vtable can stop for gc - ensure using handles
for (int i = 0; i < len; i++) {
HandleMark hm(THREAD);
assert(methods()->obj_at(i)->is_method(), "must be a methodOop");
methodHandle mh(THREAD, (methodOop)methods()->obj_at(i));
bool needs_new_entry = update_inherited_vtable(ik(), mh, super_vtable_len, checkconstraints, CHECK);
if (needs_new_entry) {
put_method_at(mh(), initialized);
mh()->set_vtable_index(initialized); // set primary vtable index
initialized++;
}
}
// add miranda methods; it will also update the value of initialized
fill_in_mirandas(initialized);
// In class hierarchies where the accessibility is not increasing (i.e., going from private ->
// package_private -> publicprotected), the vtable might actually be smaller than our initial
// calculation.
assert(initialized <= _length, "vtable initialization failed");
for(;initialized < _length; initialized++) {
put_method_at(NULL, initialized);
}
NOT_PRODUCT(verify(tty, true));
}
}
void connection_handler::handle_messages()
{
detail::handling_messages hm(handling_messages_); // reset on exit
bool bootstrapping = hpx::is_starting();
bool has_work = true;
std::size_t k = 0;
hpx::util::high_resolution_timer t;
std::list<std::pair<int, MPI_Request> > close_requests;
// We let the message handling loop spin for another 2 seconds to avoid the
// costs involved with posting it to asio
while(bootstrapping || (!stopped_ && has_work) || (!has_work && t.elapsed() < 2.0))
{
// break the loop if someone requested to pause the parcelport
if(!enable_parcel_handling_) break;
// handle all send requests
{
hpx::lcos::local::spinlock::scoped_lock l(senders_mtx_);
for(
senders_type::iterator it = senders_.begin();
!stopped_ && enable_parcel_handling_ && it != senders_.end();
/**/)
{
if((*it)->done())
{
it = senders_.erase(it);
}
else
{
++it;
}
}
has_work = !senders_.empty();
}
// Send the pending close requests
{
hpx::lcos::local::spinlock::scoped_lock l(close_mtx_);
typedef std::pair<int, int> pair_type;
BOOST_FOREACH(pair_type p, pending_close_requests_)
{
header close_request = header::close(p.first, p.second);
close_requests.push_back(std::make_pair(p.first, MPI_Request()));
MPI_Isend(
close_request.data(), // Data pointer
close_request.data_size_, // Size
close_request.type(), // MPI Datatype
close_request.rank(), // Destination
0, // Tag
communicator_, // Communicator
&close_requests.back().second
);
}
pending_close_requests_.clear();
}
// add new receive requests
std::pair<bool, header> next(acceptor_.next_header());
if(next.first)
{
boost::shared_ptr<receiver> rcv;
receivers_rank_map_type::iterator jt = receivers_map_.find(next.second.rank());
if(jt != receivers_map_.end())
{
receivers_tag_map_type::iterator kt = jt->second.find(next.second.tag());
if(kt != jt->second.end())
{
if(next.second.close_request())
{
hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
free_tags_.push_back(kt->second->tag());
jt->second.erase(kt);
if(jt->second.empty())
{
receivers_map_.erase(jt);
}
}
else
{
rcv = kt->second;
}
}
}
if(!next.second.close_request())
{
next.second.assert_valid();
if(!rcv)
{
rcv = boost::make_shared<receiver>(communicator_, get_next_tag());
}
rcv->async_read(next.second, *this);
receivers_.push_back(rcv);
}
}
// handle all receive requests
for(
receivers_type::iterator it = receivers_.begin();
!stopped_ && enable_parcel_handling_ && it != receivers_.end();
/**/)
{
if((*it)->done(*this))
{
HPX_ASSERT(
!receivers_map_[(*it)->rank()][(*it)->sender_tag()]
|| receivers_map_[(*it)->rank()][(*it)->sender_tag()].get() == it->get()
);
receivers_map_[(*it)->rank()][(*it)->sender_tag()] = *it;
it = receivers_.erase(it);
}
else
{
++it;
}
}
if(!has_work) has_work = !receivers_.empty();
// handle completed close requests
for(
std::list<std::pair<int, MPI_Request> >::iterator it = close_requests.begin();
!stopped_ && enable_parcel_handling_ && it != close_requests.end();
)
{
int completed = 0;
MPI_Status status;
int ret = 0;
ret = MPI_Test(&it->second, &completed, &status);
HPX_ASSERT(ret == MPI_SUCCESS);
if(completed && status.MPI_ERROR != MPI_ERR_PENDING)
{
hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
free_tags_.push_back(it->first);
it = close_requests.erase(it);
}
else
{
++it;
}
}
if(!has_work)
has_work = !close_requests.empty();
if (bootstrapping)
bootstrapping = hpx::is_starting();
if(has_work)
{
t.restart();
k = 0;
}
else
{
if(enable_parcel_handling_)
{
hpx::lcos::local::spinlock::yield(k);
++k;
}
}
}
void calibrate(const std::vector<TiePoint>& tiePoints,const int gridSize[2],const double tileSize[2],const unsigned int depthFrameSize[2],const unsigned int colorFrameSize[2])
{
/* Initialize the depth camera's intrinsic parameter matrix: */
Math::Matrix depthV(6,6,0.0);
/* Process all tie points: */
for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
{
/* Enter the tie point's depth homography into the intrinsic parameter matrix: */
Homography::Matrix hm(1.0);
hm(0,2)=double(depthFrameSize[0]);
hm*=tpIt->depthHom.getMatrix();
Homography::Matrix scale(1.0);
scale(0,0)=1.0/tileSize[0];
scale(1,1)=1.0/tileSize[1];
hm*=scale;
double row[3][6];
static const int is[3]= {0,0,1};
static const int js[3]= {1,0,1};
for(int r=0; r<3; ++r)
{
int i=is[r];
int j=js[r];
row[r][0]=hm(0,i)*hm(0,j);
row[r][1]=hm(0,i)*hm(1,j)+hm(1,i)*hm(0,j);
row[r][2]=hm(0,i)*hm(2,j)+hm(2,i)*hm(0,j);
row[r][3]=hm(1,i)*hm(1,j);
row[r][4]=hm(1,i)*hm(2,j)+hm(2,i)*hm(1,j);
row[r][5]=hm(2,i)*hm(2,j);
}
for(int i=0; i<6; ++i)
row[1][i]-=row[2][i];
for(int r=0; r<2; ++r)
{
for(unsigned int i=0; i<6; ++i)
for(unsigned int j=0; j<6; ++j)
depthV(i,j)+=row[r][i]*row[r][j];
}
}
/* Find the intrinsic parameter linear system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> depthQe=depthV.jacobiIteration();
unsigned int minEIndex=0;
double minE=Math::abs(depthQe.second(0));
for(unsigned int i=1; i<6; ++i)
{
if(minE>Math::abs(depthQe.second(i)))
{
minEIndex=i;
minE=Math::abs(depthQe.second(i));
}
}
std::cout<<"Smallest eigenvalue of v = "<<depthQe.second(minEIndex)<<std::endl;
/* Calculate the intrinsic parameters: */
Math::Matrix b=depthQe.first.getColumn(minEIndex);
std::cout<<b(0)<<", "<<b(1)<<", "<<b(2)<<", "<<b(3)<<", "<<b(4)<<", "<<b(5)<<std::endl;
double v0=(b(1)*b(2)-b(0)*b(4))/(b(0)*b(3)-Math::sqr(b(1)));
double lambda=b(5)-(Math::sqr(b(2))+v0*(b(1)*b(2)-b(0)*b(4)))/b(0);
double alpha=Math::sqrt(lambda/b(0));
double beta=Math::sqrt(lambda*b(0)/(b(0)*b(3)-Math::sqr(b(1))));
double gamma=-b(1)*Math::sqr(alpha)*beta/lambda;
double u0=gamma*v0/beta-b(2)*Math::sqr(alpha)/lambda;
std::cout<<"Intrinsic camera parameters:"<<std::endl;
std::cout<<alpha<<" "<<gamma<<" "<<u0<<std::endl;
std::cout<<0.0<<" "<<beta<<" "<<v0<<std::endl;
std::cout<<0.0<<" "<<0.0<<" "<<1.0<<std::endl;
/* Create the intrinsic camera parameter matrix: */
Math::Matrix a(3,3,1.0);
a.set(0,0,alpha);
a.set(0,1,gamma);
a.set(0,2,u0);
a.set(1,1,beta);
a.set(1,2,v0);
Math::Matrix aInv=a.inverse();
/* Calculate extrinsic parameters for each tie point to get measurements for the depth formula regression: */
Math::Matrix depthAta(2,2,0.0);
Math::Matrix depthAtb(2,1,0.0);
for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
{
/* Convert the tie point's depth homography to a matrix: */
Homography::Matrix hm(1.0);
hm(0,2)=double(depthFrameSize[0]);
hm*=tpIt->depthHom.getMatrix();
Homography::Matrix scale(1.0);
scale(0,0)=1.0/tileSize[0];
scale(1,1)=1.0/tileSize[1];
hm*=scale;
Math::Matrix h(3,3);
for(unsigned int i=0; i<3; ++i)
for(unsigned int j=0; j<3; ++j)
h(i,j)=hm(i,j);
/* Calculate the extrinsic parameters: */
double lambda=0.5/(aInv*h.getColumn(0)).mag()+0.5/(aInv*h.getColumn(1)).mag();
Math::Matrix r1=lambda*aInv*h.getColumn(0);
Math::Matrix r2=lambda*aInv*h.getColumn(1);
Math::Matrix r3(3,1);
for(unsigned int i=0; i<3; ++i)
r3.set(i,r1((i+1)%3)*r2((i+2)%3)-r1((i+2)%3)*r2((i+1)%3)); // 'Tis a cross product, in case you're wondering
Math::Matrix t=lambda*aInv*h.getColumn(2);
/* Create the extrinsic parameter matrix: */
Math::Matrix rt(3,4);
rt.setColumn(0,r1);
rt.setColumn(1,r2);
rt.setColumn(2,r3);
rt.setColumn(3,t);
Math::Matrix wgc(4,1);
wgc(0)=tileSize[0]*double(gridSize[0])*0.5;
wgc(1)=tileSize[1]*double(gridSize[1])*0.5;
wgc(2)=0.0;
wgc(3)=1.0;
Math::Matrix cgc=rt*wgc;
if(cgc(2)<0.0)
{
/* Flip the extrinsic matrix to move the grid to positive z: */
Math::Matrix flip(3,3,-1.0);
rt=flip*rt;
cgc=rt*wgc;
}
std::cout<<"Grid center: "<<cgc(0)<<", "<<cgc(1)<<", "<<cgc(2)<<std::endl;
/* Transform all world grid points with the extrinsic matrix to get their camera-space z values: */
for(int y=0; y<gridSize[1]; ++y)
for(int x=0; x<gridSize[0]; ++x)
{
/* Create the world point: */
Math::Matrix wp(4,1);
wp(0)=tileSize[0]*double(x);
wp(1)=tileSize[1]*double(y);
wp(2)=0.0;
wp(3)=1.0;
/* Transform the world point to camera space: */
Math::Matrix cp=rt*wp;
double dist=cp(2);
/* Get the depth frame value from the grid's plane in depth image space: */
Point dip=tpIt->depthHom.transform(Point(x,y));
const Plane::Vector& n=tpIt->gridPlane.getNormal();
double o=tpIt->gridPlane.getOffset();
double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];
/* Enter the depth / z pair into the depth formula accumulator: */
depthAta(0,0)+=1.0;
depthAta(0,1)+=-dist;
depthAta(1,0)+=-dist;
depthAta(1,1)+=dist*dist;
depthAtb(0)+=-dist*depth;
depthAtb(1)+=dist*dist*depth;
}
}
/* Solve the depth formula least-squares system: */
Math::Matrix depthX=depthAtb.divideFullPivot(depthAta);
std::cout<<"Depth conversion formula: dist = "<<depthX(0)<<" / ("<<depthX(1)<<" - depth)"<<std::endl;
/* Calculate the full depth unprojection matrix: */
Math::Matrix depthProj(4,4,0.0);
depthProj(0,0)=1.0/alpha;
depthProj(0,1)=-gamma/(alpha*beta);
depthProj(0,3)=-u0/alpha+v0*gamma/(alpha*beta);
depthProj(1,1)=1.0/beta;
depthProj(1,3)=-v0/beta;
depthProj(2,3)=-1.0;
depthProj(3,2)=-1.0/depthX(0);
depthProj(3,3)=depthX(1)/depthX(0);
Math::Matrix colorAta(12,12,0.0);
for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
{
for(int y=0; y<gridSize[1]; ++y)
for(int x=0; x<gridSize[0]; ++x)
{
/* Enter the tie point into the color calibration matrix linear system: */
Point dip=tpIt->depthHom.transform(Point(x,y));
const Plane::Vector& n=tpIt->gridPlane.getNormal();
double o=tpIt->gridPlane.getOffset();
double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];
Math::Matrix dwp(4,1);
dwp(0)=dip[0]+double(depthFrameSize[0]);
dwp(1)=dip[1];
dwp(2)=depth;
dwp(3)=1.0;
dwp=depthProj*dwp;
for(int i=0; i<3; ++i)
dwp(i)/=dwp(3);
Point cip=tpIt->colorHom.transform(Point(x,y));
cip[0]/=double(colorFrameSize[0]);
cip[1]/=double(colorFrameSize[1]);
double eq[2][12];
eq[0][0]=dwp(0);
eq[0][1]=dwp(1);
eq[0][2]=dwp(2);
eq[0][3]=1.0;
eq[0][4]=0.0;
eq[0][5]=0.0;
eq[0][6]=0.0;
eq[0][7]=0.0;
eq[0][8]=-cip[0]*dwp(0);
eq[0][9]=-cip[0]*dwp(1);
eq[0][10]=-cip[0]*dwp(2);
eq[0][11]=-cip[0];
eq[1][0]=0.0;
eq[1][1]=0.0;
eq[1][2]=0.0;
eq[1][3]=0.0;
eq[1][4]=dwp(0);
eq[1][5]=dwp(1);
eq[1][6]=dwp(2);
eq[1][7]=1.0;
eq[1][8]=-cip[1]*dwp(0);
eq[1][9]=-cip[1]*dwp(1);
eq[1][10]=-cip[1]*dwp(2);
eq[1][11]=-cip[1];
for(int row=0; row<2; ++row)
{
for(unsigned int i=0; i<12; ++i)
for(unsigned int j=0; j<12; ++j)
colorAta(i,j)+=eq[row][i]*eq[row][j];
}
}
}
/* Find the color calibration system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> colorQe=colorAta.jacobiIteration();
minEIndex=0;
minE=Math::abs(colorQe.second(0,0));
for(unsigned int i=1; i<12; ++i)
{
if(minE>Math::abs(colorQe.second(i,0)))
{
minEIndex=i;
minE=Math::abs(colorQe.second(i,0));
}
}
/* Create the normalized color homography: */
Math::Matrix colorHom(3,4);
double scale=colorQe.first(11,minEIndex);
for(int i=0; i<3; ++i)
for(int j=0; j<4; ++j)
colorHom(i,j)=colorQe.first(i*4+j,minEIndex)/scale;
/* Create the full color unprojection matrix by extending the homography: */
Math::Matrix colorProj(4,4);
for(unsigned int i=0; i<2; ++i)
for(unsigned int j=0; j<4; ++j)
colorProj(i,j)=colorHom(i,j);
for(unsigned int j=0; j<4; ++j)
colorProj(2,j)=j==2?1.0:0.0;
for(unsigned int j=0; j<4; ++j)
colorProj(3,j)=colorHom(2,j);
/* Modify the color unprojection matrix by the depth projection matrix: */
colorProj*=depthProj;
/* Write the calibration file: */
IO::FilePtr calibFile(IO::openFile("CameraCalibrationMatrices.dat",IO::File::WriteOnly));
calibFile->setEndianness(Misc::LittleEndian);
for(int i=0; i<4; ++i)
for(int j=0; j<4; ++j)
calibFile->write<double>(depthProj(i,j));
for(int i=0; i<4; ++i)
for(int j=0; j<4; ++j)
calibFile->write<double>(colorProj(i,j));
}
// Write the field information portion of ClassFile structure
// JVMSpec| u2 fields_count;
// JVMSpec| field_info fields[fields_count];
void JvmtiClassFileReconstituter::write_field_infos() {
HandleMark hm(thread());
Array<AnnotationArray*>* fields_anno = ikh()->fields_annotations();
Array<AnnotationArray*>* fields_type_anno = ikh()->fields_type_annotations();
// Compute the real number of Java fields
int java_fields = ikh()->java_fields_count();
write_u2(java_fields);
for (JavaFieldStream fs(ikh()); !fs.done(); fs.next()) {
AccessFlags access_flags = fs.access_flags();
int name_index = fs.name_index();
int signature_index = fs.signature_index();
int initial_value_index = fs.initval_index();
guarantee(name_index != 0 && signature_index != 0, "bad constant pool index for field");
// int offset = ikh()->field_offset( index );
int generic_signature_index = fs.generic_signature_index();
AnnotationArray* anno = fields_anno == NULL ? NULL : fields_anno->at(fs.index());
AnnotationArray* type_anno = fields_type_anno == NULL ? NULL : fields_type_anno->at(fs.index());
// JVMSpec| field_info {
// JVMSpec| u2 access_flags;
// JVMSpec| u2 name_index;
// JVMSpec| u2 descriptor_index;
// JVMSpec| u2 attributes_count;
// JVMSpec| attribute_info attributes[attributes_count];
// JVMSpec| }
write_u2(access_flags.as_int() & JVM_RECOGNIZED_FIELD_MODIFIERS);
write_u2(name_index);
write_u2(signature_index);
int attr_count = 0;
if (initial_value_index != 0) {
++attr_count;
}
if (access_flags.is_synthetic()) {
// ++attr_count;
}
if (generic_signature_index != 0) {
++attr_count;
}
if (anno != NULL) {
++attr_count; // has RuntimeVisibleAnnotations attribute
}
if (type_anno != NULL) {
++attr_count; // has RuntimeVisibleTypeAnnotations attribute
}
write_u2(attr_count);
if (initial_value_index != 0) {
write_attribute_name_index("ConstantValue");
write_u4(2); //length always 2
write_u2(initial_value_index);
}
if (access_flags.is_synthetic()) {
// write_synthetic_attribute();
}
if (generic_signature_index != 0) {
write_signature_attribute(generic_signature_index);
}
if (anno != NULL) {
write_annotations_attribute("RuntimeVisibleAnnotations", anno);
}
if (type_anno != NULL) {
write_annotations_attribute("RuntimeVisibleTypeAnnotations", type_anno);
}
}
}